Robotics and Autonomous Systems 00 (2011) 1–13

Robotics andAutonomous

Systems

Robust Camera Pose and Scene Structure Analysis for Service Robotics

Sorin M. Grigorescu∗, Gigel Macesanu, Tiberiu T. Cocias, Dan Puiu, Florin Moldoveanu

Department of Automation, Transilvania University of Brasov, Mihai Viteazu 5, 500174, Brasov, Romania.

Abstract

Successful path planning and object manipulation in service robotics applications rely both on a good estimation of the robot’sposition and orientation (pose) in the environment, as well as on a reliable understanding of the visualized scene. In this paper arobust real-time camera pose and scene structure estimation system is proposed. Firstly, the pose of the camera is estimated throughthe analysis of so-called tracks. The tracks include key features from the imaged scene and geometric constraints which are usedto solve the pose estimation problem. Secondly, based on the calculated pose of the camera, i.e. robot, the scene is analyzed viaa robust depth segmentation and object classification approach. In order to reliably segment the objects depth, a feedback controltechnique at image processing level has been used with the purpose of improving the robustness of the robotic vision system withrespect to external influences, such as cluttered scenes and variable illumination conditions. The control strategy detailed in thispaper is based on the traditional open-loop mathematical model of the depth estimation process. In order to control a roboticsystem, the obtained visual information is classified into objects of interest and obstacles. The proposed scene analysis architectureis evaluated through experimental results within a robotic collision avoidance system.

Keywords:Robot vision systems, Feedback control, Stereo vision, Robustness, 3D Reconstruction

1. Introduction

Integrating visual perceptual capabilities into the control ar-chitecture of a robot is not a trivial task, especially for the caseof service robots which have to work in unstructured environ-ments with variable illumination conditions [1]. As a result ofprogress in research on robot vision and technology develop-ment, the use of vision as a primary perception sensor for pro-viding information for controlling autonomous systems, suchas mobile robots and redundant manipulators, has grown sig-nificantly in recent years [1], [2]. A crucial requirement for arobot vision system is the achievement of a human-like robust-ness against the complexity of the robot’s environment in orderto provide reliable visual information for autonomous function-ing. A robot vision system is used to robustly analyze imagesacquired from complex scenes where objects to be recognizedare surrounded by a variety of other objects. As well as beingrobust against cluttered scenes, a robot vision system has to be

IThe source code for the proposed scene perception system can be found atthe SVN repository http://rovis.unitbv.ro/rovis∗Corresponding author. Tel./Fax: +40-268-418-836.Email addresses: s.grigorescu@unitbv.ro (Sorin M. Grigorescu),

gigel.macesanu@unitbv.ro (Gigel Macesanu),tiberiu.cocias@unitbv.ro (Tiberiu T. Cocias), puiudan@unitbv.ro(Dan Puiu), moldof@unitbv.ro (Florin Moldoveanu)

robust against unpredictability in the appearance of objects dueto different external influences such as variable illumination.The main requirement for such a visual system is to reliablemap the imaged scene into a virtual 3D environment that canbe used to infer the future and immediate actions of the robot.

The most common approach to 3D perception, or depth sen-sation, is through stereo vision. Basically, stereo vision ex-ploits the geometry between two perspective cameras imaging ascene. By analyzing the perspective views between the acquiredimages, 3D visual information can be extracted. Traditionally,stereo vision is implemented using a pair of calibrated cameraswith a known baseline between their optical points. Having inmind that the geometrical relations between the two camerasare known, by calculating the relative perspective projection ofobject points in both images, their 3D world coordinates canbe reconstructed until a certain accuracy [3]. The problem ofstereo camera position and orientation (pose) estimation is il-lustrated in Fig. 1. As a camera C moves through the Euclideanspace, the distances to the imaged objects, as well as the trans-lation ti and rotation Ri of Ci with respect to its previous posesshould be determined from a set of observed feature points P j.The pose of the robotic system is inherently obtained once thepose of the camera has been calculated.

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 2

C0

C1

C2

P0

P1

P2

P3

R0, t0

R1, t1

Figure 1: The camera pose (i.e. robot pose) and scene reconstruction problem.

1.1. Related WorkIn this paper, the authors propose a feedback control ap-

proach of a depth estimation system, aiming at compensatingthe problem of using constant image processing parameters incomplex environments. The objectives of the proposed archi-tecture are to reliable extract the objects of interest togetherwith the camera-objects distances, as well as the pose of thecamera while it moves through space. When feedback controltechniques are discussed in connection to robot vision, they areusually put in the context of controlling a certain system usingvisual information. Such devices are typically named Active Vi-sion [4] or Visual Servoing Systems [5]. There are relatively fewpublications dealing with control techniques applied directly onthe image processing chain.

The idea of feedback image processing has been tackledpreviously in the computer vision community in papers suchas [6] or [7]. One of the first comprehensive papers on the usageof feedback information at image processing level can be foundin [8], where reinforcement learning was used as a way to mapinput images to corresponding optimal segmentation parame-ters. In [6], a hypothesis generation and verification methodwas developed in order to calculate interest operators whichcan be used to locate target objects, such as bridges, in noisydata. Also, in [7], a feedback strategy was employed in theself-adaptation of a learning-based object recognition systemthat has to perform in variable illumination conditions. In [9],dynamic closed-loop systems were used to automatically adaptcamera parameters at the image acquisition stage. In the areaof stereo vision, probabilistic methods for the robust analysis ofdepth estimation were adopted in [10].

Although the mentioned literature is focused on closed-loopprocessing, it does not provide a suitable control frameworkfrom both the image, as well as from the control point of view.Techniques for image processing inspired from control engi-neering were used in [11] for adapting a character recognitionsystem, as well as for a quality control one. In the field of robotvision, the authors successfully used feedback control conceptsto tune region [12] and boundary [13] based segmentation oper-

ations in order to improve the visual perceptual capabilities ofa service robot [14]. In this paper, feedback machine vision isfurther investigated by proposing a closed-loop model of depthsensing based on the extremum seeking control paradigm setforth in [15]. Its use is demonstrated in the vision system pro-posed in this paper.

Camera motion estimation, or egomotion, has been studiedwithin the Simultaneous Localization and Mapping (SLAM)context [16]. Using detected visual information, motion esti-mation techniques can provide a very precise egomotion of therobot. In SLAM also, the basic sensor used is the stereo cam-era [17], [18]. The main operation involved in stereo motion es-timation is the computation of so-called correspondence pointsused for calculating the 3D pose of the robot’s camera. Basedon the extracted features, the robot’s motion can be calculatedwith the help of estimators such as the Kalman [19] or ParticleFilter [20]. Although for the visual control of a robot the es-timation of its motion is crucial, it is not treated in this papersince the objective here is to obtain a robust visual perceptionof the imaged environment. Nevertheless, in order to determinethe positions and orientations of the visualized objects with re-spect to the robot, its pose has been obtained via correspon-dence points matching, thus neglecting dynamic measures suchas the robot’s velocities and accelerations. Also, the robot’spose is needed for fusing the obtained disparity maps in orderto construct a virtual 3D model of the scene.

1.2. Structure and Main Contributions

The main contributions of the presented paper may be sum-marized as follows:

1. improvement of the depth estimation process through theinclusion of a feedback control technique at the depthmap computation level;

2. fusion of closed-loop depth computation with camera poseestimation.

This paper is organized as follows. In Section 2, the pro-posed theory behind feedback modeling of image processingsystems is presented, followed in Section 3 by a description ofthe visual architecture and information flow within the visionsystem. In Section 4, the estimation of the imaged scene ge-ometry using the novel closed-loop depth sensing algorithm isdetailed, along with the fusion of the calculated depth maps. InSection 5, the recognition of objects of interest and obstacles,based on the obtained depth information and 2D feature extrac-tion, is presented. Finally, before conclusions and outlook, per-formance evaluation is given through experimental results.

2. Feedback Control in Image Processing

In a robotic application, the purpose of the image process-ing system is to understand the surrounding environment of therobot through visual information. Usually, an object recogni-tion and 3D reconstruction chain for robot vision consists oflow and high levels of processing operations. Low level imageprocessing deals with pixel wise operations aiming to improve

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 3

Input Intensity Image

Double Thresholding

Segmentation Evaluation

New thresholding parameters

Initial thresholding

parameters

Input Image

Image Processing

Operation (u)

Image Processing

Quality Measure

y

Initial processing

parameters

Figure 2: Feedback control of an image processing operation.

the input images and also separate objects of interest from back-ground. Both the inputs and outputs of the low level processingblocks are images. The second type of modules, which dealwith high level visual information, are connected to low leveloperations through a feature extraction component which con-verts the input images to abstract data describing the imagedobjects. The importance of the quality of results coming fromlow level stages is related to the requirements of high level im-age processing. Namely, in order to obtain a proper 3D virtualreconstruction of the imaged environment at a high level stage,the inputs coming from low level have to be reliable.

Traditionally, vision systems are open-loop sequential op-erations, which function with constant predefined parametersand have no interconnections between them. This approach hasimpact on the final 3D reconstruction result, since each oper-ation in the chain is applied sequentially, with no informationbetween the different levels of processing. In other words, lowlevel image processing is performed regardless of the require-ments of high level processing. In such a system, for example,if the segmentation module fails to provide a good output, allthe subsequent steps will fail.

The basic diagram from which feedback mechanisms formachine vision are derived can be seen in Fig. 2. In such acontrol system, the control signal u, or actuator variable, is aparameter of an image processing operation, whereas the con-trolled, or state, variable y is a measure of processing quality.

The design and implementation of feedback structures inmachine vision is significantly different from conventional in-dustrial control applications, especially in the selection of thepair actuator variable - controlled / state variable. The choiceof this pair has to be appropriate from the control, as well asfrom the image processing point of view.

In order to derive a control strategy for a machine visionsystem, the following discrete nonlinear state-space representa-tion model of the vision apparatus is suggested:x(k) = f [x(k),u(k)],

y(k) = g[x(k)],(1)

where x ∈ <n is the state vector, u ∈ < is the actuator (input),y ∈ < is the output vector, f : <n × < → <n is the statetransition function and g : <n → < is the output function. krepresents the discrete time. Suppose that we have a controllaw:

u(k) = α[x(k), θ], (2)

the control problem is to find the optimal parameter θ∗ whichprovides an output of desired, or reference, quality. Following

the above reasoning, the closed-loop system:

x = f [x, α(x, θ)] (3)

has its equilibrium point parameterized by θ. Having in mindthe high non-linearity of an image processing system, a controlstrategy based on extremum seeking [15] is suggested. Thus,the goal of the feedback control system is to determine the op-timal parameter θ∗ as the minimum, or maximum, value of thestate vector x:

θ∗ = arg min x(k) or θ∗ = arg max x(k). (4)

The choice of this particular type of control method lies inthe fact that, taking into account the non-linearity of an imageprocessing system, it is difficult to determine reference valuesthat could be applied to classical feedback structures. Hence,in the image processing control approach, the desired state of avision system is given by the extremal values of the state vector.In the following, the proposed model is applied to the depthestimation processed detailed in the next section.

3. Visual Architecture Overview

The objective of the proposed visual understanding systemis to robustly estimate, in real-time, the structure of the imagedscene with respect to the pose of the camera in space. Having inmind the large amount of information that has to be processed,the first step in the development of the scene understanding sys-tem is to model the flow of information into a visual architec-ture. In Fig. 3, the block diagram of the visual understandingsystem can be seen. Basically, the overall architecture has beendivided into two main components, that is, the scene geometryestimation and the scene understanding modules, both of themexplained in the next sections.

The goal of the scene geometry estimation component is todetermine the pose of the camera while it moves. As shownin Fig. 3, this procedure is performed using information ex-tracted from stereo images and grouped into so-called tracks. Atrack represents the visual data calculated from a pair of stereoimages, as well as the camera’s a-priori known geometry (e.g.baseline between the two optical sensors) and internal param-eters (e.g. focal length, optical center, etc.). In the presentedsystem, a track contains the following information:

• input stereo images IL(x, y) and IR(x, y);

• set of 2D correspondence points p j between IL(x, y) andIR(x, y);

• set of 3D correspondence points P j calculated from p j;

• stereo camera pose C;

• depth map Id(x, y);

• set of objects of interest Oint;

• set of obstacles Oo.

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 4

Scene UnderstandingScene Geometry Estimation

Stereo Image

Acquisition

Track Geometry Estimation

Depth Estimation

Camera Pose

Estimation

Object Recognition

and Classification

Complex Scene

Perception

BundleAdjustment

Right

Image

Left

Image

Figure 3: Block diagram of the proposed 3D visual perception system.

In robotic pose estimation, or more particularly camera poseestimation, the main variables that have to be determined are the3D positions of the corresponding points P j and the positionand orientation of the camera Ci.

P j =[x j y j z j

]T, (5)

Ci =[xi yi zi φi ψi θi

]T, (6)

where points P j are matched 3D points in the left image of twoconsecutive stereo images and T represents the transpose.

Once the 3D positions of the correspondence feature pointshave been determined in two adjacent stereo images, the cam-era pose can be reconstructed using triangulation, as will be ex-plained in the next section. In real world applications the com-putation of 3D feature points from their 2D correspondence issubjected to measurement noise, thus the obtained camera posemay contain errors with respect to its position and orientation.To cope with these errors, a sparse bundle adjustment tech-nique was adopted [21]. The goal of bundle adjustment is torecalculate the pose of the camera based on a minimization al-gorithm which takes into account the 3D correspondence pointsand their 2D reprojection error over a sequence of tracks. Afterthe camera’s pose has been determined, the relative distancesto the objects and obstacles can be calculated through compu-tation of so-called depth maps [22]. This step is a crucial onein the visual system, since its result influences the behavior ofthe system with respect to the scene geometry. The depth mapcalculation process, together with its closed-loop enhancement,will be described in Section 4. In Fig. 3, the feedback controlimprovement of the depth sensing component is represented bythe feedback loop illustrated with dashed line.

Having obtained the pose of the camera and the raw camera-objects/obstacles distances, the imaged scene can be analyzedwith the purpose of detecting the objects of interest and obsta-cles relative to the pose of the camera, or robot. This is accom-plished by the second main module of the proposed vision sys-tem, that is the scene understanding one. The construction ofthe module is relatively straightforward, namely, after an objectrecognition and classification procedure, the semantics of the

environment are determined based on the context of the scene,as described in Section 5.

4. Scene Geometry Estimation

One important problem to be solved in robot localizationand scene understanding is the estimation of the camera’s posein the Cartesian space, along with the geometry of the imagedenvironment, that is, the camera-objects distances. In orderto solve these problems, two types of depth calculation ap-proached have been developed. The first one aims at deter-mining correspondence points between two consecutive stereoimages pairs with the purpose of obtaining the relative poses ofthe cameras with respect to each stereo pair. The second depthcalculation method has as goal the estimation of the scene struc-ture with respect to the already calculated poses of the camera.The two approaches have been named sparse and dense depthestimation, respectively.

4.1. Camera and Measurement Models

The model of the stereo camera used in sensing the robot’senvironment is illustrated in Fig. 4. A real world point rep-resented in homogeneous coordinates P =

[X Y Z 1

]Tis

projected onto the image planes of a stereo camera as the ho-mogeneous 2D image points:pL =

[xL yL 1

]T,

pR =[xR yR 1

]T,

(7)

where pL and pR have the 2D coordinates (xL, yL) and (xR, yR)projected onto the left IL and right IR images, respectively. ThepL and pR 2D image positions are given by the intersection withthe image plane of the line connecting point P in world coor-dinates with the optical centers OL and OR of both cameras, asshown in Fig. 4. The image, or principal plane, is located at adistance f from the optical center of a camera. f is commonlyknown as the focal length. The z axis of the coordinate systemattached to the optical center is referred to as the principal ray,or optical axis. The principal ray intersects the image plane atimage center (cx, cy), also known as the principal point. The

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 5

Camerastângă

Cameradreaptă

X

xL=QL·X xR=QR·X

P

OL OR

f

(cx, cy)

xL

yL

xR

yR

Z

T

x

y

x

y

Pri

ncip

al r

ay

pL

pR

dmax dmin

HIL IR

Figure 4: Principle of depth estimation of a point P on a pair of rectified andundistorted stereo images.

origin of the image coordinate system is defined as the imagetop-left corner (x0, y0).

Knowing pL, pR and the distance T between the optical cen-ters of the two cameras, the distance, or depth, Z from the stereocamera to point P can be calculated, thus obtaining the 3D po-sition of P with respect to the camera. Having in mind the per-spective projection of P onto the image planes, given by pL andpR, the 3D position of P is determined using the next three for-mulas:

X = xL ·Td, (8)

Y = yL ·Td, (9)

Z = f ·Td, (10)

where d is the disparity of the projected point P:

d = xL − xR. (11)

From Eq. 10 it can be observed that the distance is inverselyproportional to the disparity. Since we have considered rectifiedimages as inputs, that is, images with parallel rows, the dispar-ity d is given only by the difference between the point coordi-nates on the x image axis.

4.2. Sparse Feature Points Depth Estimation

The goal of 3D depth estimation for feature points in con-secutive stereo images is to obtain a relationship between theposes of the stereo camera. This relation is established basedon the principle illustrated in Fig. 5(a). Namely, the new cam-era pose C(k + 1) is determined with respect to the previousone C(k) by evaluating the correspondence points between theleft and right stereo images and between the left images cor-responding to C(k) and C(k + 1). The pose of the right imagesensor differs from the left one only along the x position of the

IR(k+1)xL

yL IL(k+1)

xy

z x+T

IR(k)IL(k)

C(k+1)=R·C(k) + t

***

*** *

* Pj

(a)

(b) (c)

Figure 5: Camera pose estimation through correspondence calculation. (a)Consecutive pose estimation principle (b) 2D feature points calculation. (c)3D feature points and camera pose estimation.

Cartesian space. This difference is represented by the baselineT between the two sensors of the stereo camera.

Firstly, for camera pose estimation, the 2D correspondencepoints p between the left and right stereo images are calculated.2D feature points have been extracted via the Harris corner de-tector [23], followed by a correspondence matching using a tra-ditional cross-correlation similarity measure [24]. Their corre-sponding 3D positions are obtained using Eq. 8, 9 and 10. Sec-ondly, a matching is performed between the 2D feature pointsin consecutive stereo images, that is between images acquiredunder camera poses C(k) and C(k + 1). As convention, thesematches are calculated for the left camera only. Knowing the3D positions of the 2D points matched between adjacent im-ages, the pose of the camera can be calculated through a Perspective-N-Point (PNP) algorithm [3]. By solving the PNP problem,the rotation R and translation t matrices that relate the camera’sposes are obtained:

C(k + 1) = R ·C(k) + t. (12)

In order to solve the PNP problem, a minimum number of7 correspondence points between C(k) and C(k + 1) have tobe calculated. In Fig. 5(b), and example of 2D feature pointsextraction and their calculated 3D positions is illustrated. Usingthe feature points and the above explained principle, the poseof the camera can be estimated, as seen in Fig. 5(c). As will beexplained in Section 4.4, the obtained poses will be used for thefusion of the calculated depth maps.

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 6

4.3. Dense Closed-loop Depth Estimation

Once the pose of the camera has been determined, the 3Dstructure of the imaged scene can be reconstructed with respectto the position and orientation of the camera, i.e. with respectto the robot’s pose.

In order to properly compute the camera-object distanceZ, it is needed to establish the location of each 2D point, orpixel, p(x, y) in each stereo camera image, namely, the 2D im-age points pL and pR. The correspondence problem is currentlyone of the most investigated issues in stereo vision. In literature,there are a number of dense correspondence calculation meth-ods, a comprehensive classification being available in [22]. Inthis paper, we have chosen to control the so-called Block Match-ing (BM) algorithm with the goal to obtain reliable 3D sceneinformation.

BM is one of the most popular correspondence matching al-gorithm used in robotics, its main advantage being the fast com-putation rate, in comparison to more advanced techniques suchas Graph-Cuts (GC) [22] or Semi-Global-Matching (SGM) [25].Although BM has certain sensitivity to illumination conditions,its computation property makes the method a good candidatefor real-time autonomous systems. In this paper, points arematched by calculating a Sum of Absolute Differences (SAD)over small sliding windows. The BM method is commonly per-formed in three steps. In our implementation we have consid-ered the following operations:

• Pre-filter the input images with a 7x7 sliding window,containing a moving average filter, to reduce lighting dif-ferences and enhance texture;

• Compute SAD over a sliding window;

• Eliminate bad correspondence matches through post-filtering.

As shown in Fig. 4, the SAD values are calculated using awindow shifted in the right images along the interval:

H = [dmin, dmax], (13)

where H is referred to as the horopter, defined as the 3D vol-ume covered by the search range of BM. The goal of computingSAD is to find the best matching candidate of point pL in theright image, that is pR, as:

m =∑x,y

[IL(x, y) − IR(x + d, y)], (14)

where m is the SAD, or match, value and d ∈ H. By calculatingSAD over H, we obtain a characteristic in which its maximumrepresents the best match candidate of pL in the right image.Because of the linearity of the equation, SAD is a faster com-putational approach to BM, as opposed to other metrics such asthe Zero Mean Normalized Cross-Correlation (ZNCC), or theSum of Squared Differences (SSD) [22].

Post-filtering aims at preventing false matches, hence falsedisparity maps. For filtering bad matches, a uniqueness ratiofunction is used, defined as:

(a) (b)

Figure 6: Depth estimation via block matching. (a) Disparity map obtainedwith qr = 16. (b) 3D reprojected disparity map.

qr =(m − mmin)

mmin, (15)

where mmin is the minimum SAD, or match, value. A feature isconsidered a match if:

qr > Tq, (16)

where Tq is a predefined uniqueness threshold value. In [26],the value of the uniqueness threshold is suggested to be Tq =

12. As it will be shown latter in this section, a predefined con-stant value of Tq poses problems in 3D reconstruction since, de-pending on the imaged scene, it can introduce a large numberof outliers in the reconstructed 3D model, or a too few numberof voxels. To overcome this problem, we propose a feedbackcontrol method for the uniqueness threshold Tq. The output ofBM is a grey level image Id(x, y), also referred to as the dispar-ity map, where the levels of grey represent different distances.In Fig. 6(a), the disparity map calculated for the typical clut-tered service robotics scene from Fig. 5(b) is presented. Thepixels from Fig. 6(a) with a higher brightness are considered tobe closer to the camera. Also, the pixels for which no corre-spondence could be calculated are represented as white. The3D reprojected scene is illustrated in Fig. 6(b).

The main problem with the open-loop depth estimation sys-tem described previously is its low performance with respect tovariations in the scene structure, such as variable illuminationconditions or clutter. An example of using constant parametersof depth estimation is illustrated in Fig. 6 and 7. As said before,one of the main factors that influences the depth estimation pro-cess is the threshold value Tq of the uniqueness ration qr. If Tq

has an optimal predetermined value, as in Fig. 6(a), the recon-struction from Fig. 6(b) is fairly reliable, having in mind thatwe operate only with a pair of images. On the other hand, if thescene parameters change, or Tq has a suboptimal value, 3D re-construction might fail, as shown in Fig. 7. For a large value ofTq, as in Fig. 7(a,c), the 3D results have a low number of objectvoxels, whereas for a high Tq the number of obtained outliers istoo large, as in Fig. 7(b,d).

Although there are a number of parameters that could becontrolled, we have considered the depth estimation process,for simplicity, as a Single Input Single Output (SISO) model.

The depth sensing process has been modeled as the nonlin-ear system from Eq. 1. For the sake of clarity, the state vec-tor x is considered to have only one element which describes

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 7

(a) (b)

(c) (d)

Figure 7: 3D reconstruction results from suboptimal values of Tq. (a) Tq = 68.(b) Tq = 4. (c,d) 3D reprojected scenes.

(a) (b) (c)

Figure 8: Depth segmentation using H = [0.7m, 1.5m] and different uniquenessthresholds. (a) Optimal qr = 16. (b) Over-segmented qr = 68. (c) Under-segmented qr = 4.

the behavior of the modeled process. Since, depending on thechosen uniqueness threshold Tq, we obtain a different disparitymap Id, as shown in Fig. 7, a straightforward way to derive astate variable for the system is to quantify Id. In this paper, wesuggest the quantification of Id through distance, or depth, seg-mentation. A segmented distance is represented by the regionthresholded image Ith(x, y) obtained from the segmentation ofthe disparity map Id.

Depth segmentation can be implemented by specifying arange interval of interest H, also entitled horopter, where thedesired objects reside. Using Eq. 8-10, the horopter can betranslated from real world metric units to pixel values that mapdepth in the disparity image Id. In Fig. 8, three segmenta-tion examples using a horopter H = [0.7m, 1.5m] and differentuniqueness thresholds are illustrated. As can be seen, only thesegmentation result from Fig. 8(a) corresponds to optimal seg-mentation, the other two being either over- or under-segmented.

Using the above described depth segmentation principle basedon region segmentation, the problem of controlling the qualityof the disparity map Id is converted into the problem of control-ling the quality of the segmented image Ith. A region segmentedimage is said to be of good quality if it contains all pixels of theobjects of interest forming a “full” (unbroken) and well shapedsegmented object region. Bearing in mind the qualitative def-inition of a segmented image of good quality, the quantitativemeasure of segmented quality in Eq. 17 has been used:

im = −log2 p8, im(0) = 0, (17)

0 5 10 15 20 25 300

0.5

1

Uniqueness threshold Tq

Unc

erta

inty

Mea

sure

i m

Figure 9: The uncertainty measure im of segmented pixels vs. uniquenessthreshold Tq.

where p8 is the relative frequency, that is, the estimate of theprobability of a segmented pixel to be surrounded with 8 seg-mented pixels in its 8-pixel neighborhood:

p8 =no. of seg. px. surrounded with 8 seg. px.

total no. of seg. px. in the image. (18)

Keeping in mind that a well segmented image contains a“full” (without holes) segmented object region, it is evidentfrom Eq. 18 that a small probability p8 corresponds to a largedisorder in a binary segmented image. In this case, a large un-certainty im is assigned to the segmented image. Therefore, thegoal is to achieve a binary image having an uncertainty measureim as small as possible in order to get a reliable depth segmen-tation result.

The depth estimation system was modeled according to Eq. 1,where the involved variables are:

x =[im qr

]T, (19)

y = Id(x, y), (20)

u = qr(im,Tq). (21)

In Fig. 9, the input-output (I/O) relation between the statevariable im and the actuator parameter Tq is displayed for thecase of the scene from Fig. 6. The goal of the proposed ex-tremum seeking control system is to determine the optimal valueT ∗q which corresponds to the minimum of the curve in Fig. 9.T ∗q represents the desired value of the uniqueness threshold.The shape of the obtained I/O curves, as can also be seen fromFig. 9, preserve the controllability of the system, since the valueof the actuator converges to the global minimum representingthe equilibrium set-point of the considered system.

Following the above presented discussion, the block dia-gram of the proposed depth sensing system is illustrated in Fig. 10.Firstly, left and right images are processed in order to establishan initial depth map. The core of the method is represented bythe state feedback loop which is used to automatically adapt theactuator parameter Tq in order to obtain consistent depth esti-mation. Once the equilibrium set-point has been achieved, thecalculated Id is used to reconstruct the viewed scene in a 3Denvironment by reprojecting the voxels using Eq. 8-10.

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 8

Control Based on

Extremum Seeking

Current Image

Quality Measure

Calculation

u

Processing parameter

Output processed

image

Measure of image quality y

uopt

y

u

Image processing operation

Left image

Right imageDepth

Estimation(Disparity Map)

Disparity Segmentation

Uncertainty Measure

Calculation

3D Scene Reconstruction(Reprojection)

im

Extremum seeking

Tq*

im

Tq

Figure 10: Block diagram of the proposed feedback control system for robust depth estimation.

(a) (b) (c) (d)

Figure 11: Depth maps fusion example. (a-c) Snapshots of the imaged environment. (d) Annotated 3D model.

Although the proposed feedback depth estimation methodhas been developed around the SAD approach, its adaptationto other metrics, such as ZNCC or SSD, is direct since onlythe matching cost function is changed, the other BM parame-ters remaining the same. The feedback improvement of moreadvanced correspondence matching algorithms, such as GC orSGM, should be investigated, their research being out of thescope of this paper.

4.4. Depth Maps Fusion

Having obtained both the location of the robotic systemthrough camera pose estimation and the optimal depth mapscorresponding to each camera pose, a virtual 3D model of theimaged environment can be reconstructed by fusing these twopieces of information together [27].

Basically, the concept for this fusion problem is to projecteach calculated depth map in a virtual 3D space where eachvoxel’s 3D location is related to its corresponding estimatedcamera pose. The advantage of using the closed-loop calcu-lated depth images is that the annotated 3D model contains lessnoise, since the noisy voxels are filtered out by the feedbackadaptation algorithm.

One major problem that has to be solved in depth map fu-sion is the redundant information coming from overlapping pro-jected disparity images. In order to save computation time, wehave considered as valid voxels those ones visible in the newestimages acquired from the stereo camera. In case newer vox-els overlay voxels from previous camera poses, the older onesare discarded from the 3D model. Although this approach mayseem as a brute force one, it provides enough accurate collisiondetection results for an autonomous service robot. An exampleof depth maps fusion within a robotic scene is given in Fig. 11.

The obtained annotated 3D model contains crucial infor-mation for autonomous service robots which have to navigateor manipulate objects in complex and uncertain environments.As will be shown in Section 6.3, the 3D virtual scene can beused to detect obstacles and plan the movements of a redundantautonomous manipulator.

5. Scene Understanding

The final stage in the visual architecture proposed in this pa-per is the recognition and classification of the visualized objectsof interest and obstacles. For this purpose, a classical MinimumDistance Classifier [24] has been used. The features used tobuild the feature vectors are composed of the distance obtainedfrom the method described in Section 4, the color of the ob-jects extracted from the HSV (Hue, Value, Saturation) planeof the acquired images and the invariant moments of the colorsegmented objects.

The 2D segmentation of the objects has been performed onthe left image of the acquired stereo image pair using the robustcolor segmentation algorithm described in [12] for the case ofuniform colored objects. Also, for textured objects, such asbook, the boundary detection method from [13] has been used.The main objective of the segmentation procedure is to deter-mine the class of objects and their key points of interest. De-pending on the obtained class, in can be inferred whether or notthe object can be manipulated (e.g. bottle, glass, book, etc.).Once this manipulative feature is computed, key points are cal-culated with the goal to be used for the visual guidance of arobotic system. As will be described in the next section, sucha system can be a redundant manipulator which has the task ofobject grasping.

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 9

(a) (b)

Figure 12: Region and boundary based object recognition. (a) Input image. (b)Recognized bottle and book objects.

In Fig. 12, an example of object segmentation, classificationand key feature points extraction can be seen. The key pointsare represented in Fig. 12(b) by the bold blue and green points.As can be seen, depending on the object class, either region orboundary based segmentation has been used for its detection.

6. Performance Evaluation

In order to test the capabilities of the proposed visual sys-tem, the performance evaluation procedure has been dividedinto three parts. Firstly, a comparison between the proposedclosed-loop depth estimation algorithm and an open-loop coun-terpart is given. Secondly, the overall 3D machine vision archi-tecture is evaluated with respect to object pose detection. Fi-nally, the advantage of using the vision platform for the visualcontrol of a redundant manipulator arm is presented in the con-text of obstacle avoidance path planning.

The evaluation procedure involved a number of 500 im-ages containing 35 objects of interest, such as bottles, glasses,or books, acquired using a Bumblebeer pre-calibrated stereocamera working at a rate of 16 Frames Per Second (FPS). Anexample of such a test scene is illustrated in Fig. 5(b). The illu-mination used ranged in the interval [15lx, 1200lx]. This rangeof illumination corresponds to a variation of the light intensityfrom a dark room lighted with candles (15lx) to the lightinglevel of an office and above. According to the European lawUNI EN 12464, the optimal lighting level of an office has avalue of 500lx.

6.1. Closed-loop vs. Open-loop Depth Estimation

In order to give a qualitative evaluation of the proposedclosed-loop depth computation algorithm, the method has beencompared with the traditional approach which uses constant pa-rameters for the calculation of the disparity map. As explainedbefore, a good depth map is one that contains dense, or “full”,disparity areas with a minimum amount of noise, as in Fig. 6.In this work, the noise is represented by small disparity areaswhich perturb the 3D model, as in the example from Fig. 7(b).On the other hand, a depth map should contain as many “full”areas as possible, that is, to maximize the amount of processed3D visual information.

100 200 300 4005

10

15

20

25

30

Sample images

Tq

vari

atio

n

Closed−loop TqConstant Tq

(a)

100 200 300 4000

20

40

60

80

100

120

Sample images

SNR

(b)

Figure 13: Closed-loop vs. open-loop disparity computation. (a) Variation ofthe uniqueness threshold Tq. (b) Signal-to-Noise-Ratio of disparity areas.

Mathematically, the depth map quality index can be ex-pressed as a Signal-to-Noise-Ratio (SNR) between the total sumof pixels in disparity areas Atotal and the sum of pixels in noisyareas Anoisy:

S NR =Atotal

Anoisy, (22)

where an area is considered to be noisy if it has a value lowerthan a specific threshold. The threshold has been set to theheuristically determined value of 100px. The extraction of theareas was performed via a pixels connectivity evaluation [24].

The closed-loop variation of the uniqueness threshold Tq isrepresented in Fig. 13(a), opposed to a manually chosen con-stant value of Tq = 16. The corresponding SNR diagram isillustrated in Fig. 13(b). As can be seen from the diagram, theSNR index has a larger value for the case of closed-loop depthestimation, as for the constant value of Tq. The improvement ofthe 3D model is also visible through the mean value of the SNR,which for closed-loop has a value of 59.2105 in comparison to41.6336, representing to open-loop situation.

In a robotic application, the main advantage of the closed-loop depth estimation system is the noise reduced annotated 3Dmodel which can be used for on-line obstacle avoidance. Sucha path planning example will be given in Section 6.3 for thecase of a redundant manipulator arm.

6.2. Evaluation of the Overall Vision ArchitectureThe evaluation of the overall machine visual system has

been performed with respect to the real 3D poses of the objectsof interest. The real 3D positions and orientations of the ob-jects of interest were manually determined using the following

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 10

Xc

Xm

Yc

Ym

Zc

Zm

c

m

c

m

c

m

100 200 300 400

0

1

2

3

Sample images

Cam

. and

mar

ker

posi

tion

[m]

(a)Xc

Xm

Yc

Ym

Zc

Zm

c

m

c

m

c

m

100 200 300 400−100

−50

0

50

100

Sample images

Cam

. and

mar

ker

orie

nt. [

deg]

(b)

Figure 14: Estimated positions (a) and orientations (b) of the stereo camera.

setup. On the imaged scene, a visual marker, considered to bethe ground truth information, was installed in such a way thatthe poses of the objects could be easily measured with respect tothe marker. The 3D pose of the marker was detected using theARToolKit library which provides subpixel accuracy estimationof the marker’s location with an average error of ≈ 5mm [28].By calculating the marker’s 3D pose, a ground truth referencevalue for camera position and orientation estimation could beobtained using the inverse of the marker’s pose matrix. Further,the positions of the imaged objects, as well as the camera pose,were calculated using the proposed system which includes thefeedback mechanisms for depth estimation. Both results, thatis camera and objects poses, were compared to the ground truthdata provided by the ARToolKit marker.

In Fig. 14, camera pose estimation results obtained usingthe two methods, that is through the proposed method and viathe ARToolKit marker approach, are presented. As can be seenfrom both diagrams, the marker-less pose estimation algorithmdescribed in this paper delivered a camera position (Xc,Yc,Zc)closely related to the ground truth values (Xm,Ym,Zm) obtainedfrom the marker. Also, the calculated orientations (φc, ψc, θc)followed the reference ones (φm, ψm, θm). This correlation canbe easily observed when analysing the statistical error results,given in Tab. 1, between the two approaches. Namely, for boththe position and orientation, the errors are small enough to en-sure a good spatial localisation of the robot and also to providereliable depth maps fusion.

Table 2: Statistical results of 3D position estimation errors for the consideredobjects of interest.

Xe [m] Ye [m] Ze [m]Max error 0.3006 0.2227 0.1074Mean 0.0443 0.0264 0.0059Std. dev. 0.1190 0.1082 0.1198

In order for a robotic system to manipulate objects of inter-est, their 3D pose has to be precisely determined. As for thecase of camera position and orientation evaluation, the pose ofthe objects has been also determined with respect to the groundtruth marker. In Fig. 15 the estimated positions of the imagedobjects of interest are shown together with their real positionsmeasured with respect to the marker. Since only the positionhas been varied, the orientation remaining constant, the dia-grams in Fig. 15 illustrate only different objects positions withrespect to the pose of the camera system. Statistical results of3D position estimation errors are given in Tab. 2.

As can be seen from Tab. 2, the mean 3D position errorhas a value of (0.0443m, 0.0264m, 0.0059m) which is in manycases tolerable for object manipulation. Nevertheless, the max-imum achieved error, at image sample 204 has a high value of(0.3006m, 0.2227m, 0.1074m). Although the pose of the cameraat that particular sample has a small 3D error, the position of theobject detected at that instance is unreliable for robotic manip-ulation. It is interesting to notice that the object position erroris correlated to the camera-object distance along the Z Carte-sian axis. As it can be seen from Fig. 15(a), the Z position ofthe camera at sample 204 is at its highest value of 2.78m fromthe considered object. The high object position error comesin this case from the object triangulation algorithm. Namely,a small error in manipulative key points calculation, explainedin Section 5, exponentially increases the final 3D object posi-tion error when the camera-object distance increases. Since ina robotic system the camera is attached to the robotic platform,the objects of interest are usually imaged in a close range forthe purpose of object manipulation.

6.3. Visual Control of a Redundant Manipulator

The visual architecture proposed in this paper was also suc-cessfully used within a robotic collision avoidance system. Inorder to avoid collisions in a dynamic scene, a robotic systemhas to reliably detect the poses of objects and obstacles suchthat it corrects its movement in real-time [29]. For demonstrat-ing the collision avoidance capabilities, we have chosen to treatall detected objects as obstacles. Thus, the goal of the robot isto maintain a certain kinematic configuration in the Cartesianspace based on acquired visual information.

The robotic system has been modelled using a 7 Degrees-of-Freedom (DoF) manipulator arm in a Denavit-Hartenberg con-figuration, as shown in Fig. 16. The arm used for experimentsis a Robotnikr manipulator which is a part of the RESCUERr

robotic platform. The camera system is located at a fixed poseC with respect to the base of the manipulator arm.

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 11

Table 1: Statistical results of errors between proposed and marker based 3D camera pose estimation.

Xe [m] Ye [m] Ze [m] φm [deg] ψm [deg] θm [deg]Max error 0.0497 0.0595 0.1015 4.2348 5.6915 10.1187Mean 0.0135 0.0140 0.0429 0.7887 0.7247 0.6765Std. dev. 0.0211 0.0207 0.0642 2.3029 2.6090 5.5249

100 200 300 400−1

0

1

2

3

Sample images

X [

m]

ReferenceObject

100 200 300 400−1

0

1

2

3

Sample images

Y [

m]

100 200 300 400−1

0

1

2

3

Sample images

Z [

m]

(a) (b) (c)

Figure 15: Real (reference) and estimated positions of objects of interest along the three Cartesian axes (X,Y,Z).

010

2030

40

-15

-10

-5

0

5-10

0

10

20

30

‘`

0 10 20 30 40-20

-10

0

10

-5

0

5

10

15

20

25

Y

Z

Obstacle

Moving direction

Position and orientation maintained

Only position or orientation keept constant

TCP

qa

qb

d1

d2

Base

Pc

X

Y

Z

C

Figure 16: Experimental setup for real-time colision avoidance.

In our implementation, the collision avoidance system hasbeen configured as two algorithms. The first one keeps both theposition and the orientation of the Tool Center Point (TCP) con-stant, while the second one maintains constant only the positionor the orientation. This second case is represented by such col-lisions that cannot be avoided if the arm’s pose is kept constant.Both approaches are illustrated in Fig. 16, where the objectiveof the robot control system is to hold the pose of the graspedobject while an obstacle moves in the direction of the arm.

In order to determine a collision free configuration for therobotic arm, the control method calculates the distance d1 be-tween the surface normal of the detected object to the so-calledcollision point Pc. Pc is represented by the point on the manip-ulator arm nearest to the object. Further, we have consideredthe variable d2 represented by the distance between the TCPand the joint closest to Pc, starting from the base of the roboticarm. The collision avoidance implementation is based on thefollowing two constraints:

1. maximize the distance d1;2. maintain a constant distance d2.

During on-line operation, the system determines the twojoints qa and qb that must be controlled in order to maximized1 and keep d2 constant. These joints are obtained by evalu-ating their impact, if actuated, on the variation of d1 and d2.Mathematically, the variation is expressed as the derivative ofthe two considered distances, d1 and d2. Hence, in order to de-termine qa and qb, the collision avoidance algorithm optimizesthe following criterias:[

qa

qb

]=

arg max d1,

arg min d2.(23)

The desired positions of the joints between the base of therobot and the collision point are determined by solving the sys-tem: d1(q) = d∗1,

d2(q) = d∗2,(24)

where d∗1 is the reference safe distance from the detected objectto the robot arm and d∗2 the initial distance between the TCP andthe joint nearest to Pc, starting from the base of the manipulator.The vector q is defined as:

q =[qi

a qib

]T, (25)

where i is the number of joints of the manipulator arm. Further,the desired positions of the other robotic joints are calculatedusing the considered inverse kinematic model. The solution ofEq. 24 is determined through the following recursive equation:

qi+1 = qi + J−1(qi) · δF(qi), (26)

where:

δF(qi) = F(qi) − F∗, (27)

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 12

F(qi) =[d1(qi) d2(qi)

]T, (28)

F∗ =[d∗1 d∗2

]T, (29)

J(qi) =

∂d1∂qa

∂d2∂qa

∂d1∂qb

∂d2∂qb

. (30)

where F is the system matrix of Eq. 24, F∗ is the solution of Fand J represents the Jacobian of F.

In Fig. 17, an example of collision avoidance using the pro-posed method is illustrated. In the presented example, at timet = 0.9s, an obstacle is detected. The control method then trig-gers the system to recalculate the desired joints angles, depend-ing on the direction of the obstacle, as described above. Sincethe TCP’s pose must be maintained, only the first three joints ofthe manipulator are controlled, the other four being feed witha constant reference angle. As can be seen, the real joints val-ues qi follow the desired reference values q∗i . Also, as shownin Fig. 17(b), the real position (X,Y,Z) of the TCP is also con-sistent with their desired values (X∗,Y∗,Z∗). Finally, it can beseen from Fig. 17(c) that the cumulated TCP position error islow, with a maximum value below 5mm.

The behaviour of the robot with respect to the obstacle dis-tance d1 is illustrated in Fig. 18. In order to maintain a safe dis-tance between the robot and the detected object, or obstacle, thevalue of d1 should be keep above a predefined safety distanced∗1. As can be seen from Fig. 18, once the probability of a col-lision has been determined at t = 0.9s, the collision avoidancesystem starts controlling the manipulator arm with the goal toincrease d1 above the value of d∗1. In our experiments, we haveconsidered the minimal safety distance to be d∗1 = 150mm.

7. Conclusions and Outlook

In this paper, a robust visual perception system for servicerobotics applications has been proposed. The goal of the ar-chitecture is the reliable extraction of pose and depth informa-tion that can be used in autonomous systems, such as mobilemanipulators. The main feature of the visual platform is theclosed-loop improvement of the depth sensing system. The ro-bust depth estimation approach plays a crucial role in scene re-construction tasks where the correct detection of objects of in-terest and obstacles is needed. The proposed system has beensuccessfully tested within a collision avoidance architecture fora 7-DoF robotic manipulator. As future work, the authors con-sider the extension of the theoretical aspects of closed-loop im-age processing to other visual tasks, such as robust object recog-nition and classification. Also, the aspects of camera motiondynamics and visual information fusion will be further inves-tigated for the improvement of the proposed visual perceptionsystem.

0 1 2 3 4

−1.2

−1

−0.8

−0.6

Time [sec]

Join

t ang

les

[rad

]

q1q2q3q∗1q∗2q∗3

(a)

0 1 2 3 4−300

−200

−100

0

100

Time [sec]

TC

P p

ositi

on [m

m]

XYZX∗Y ∗Z∗

(b)

0 1 2 3 40

1

2

3

4

5

Time [sec]

TC

P po

sitio

n er

ror

[mm

]

(c)

Figure 17: Performance evaluation results for the proposed collision avoidancesystem, in a 7-DoF manipulator arm. (a) First three reference q∗i and real qijoint angles. (b) Reference (X,Y,Z) and real (X∗,Y∗,Z∗) position of the TCP.(c) Cumulated position error of the TCP.

0 1 2 3 4

100

150

200

Time [sec]

Join

t−O

bsta

cle

dist

ance

[mm

]

Controlled distance d1Reference distance d∗1

Figure 18: Robot-obstacle distance behavior during experimental evaluation.

S.M. Grigorescu et al. / Robotics and Autonomous Systems 00 (2011) 1–13 13

Acknowledgment

This paper is supported by the Sectoral Operational Pro-gram Human Resources Development (SOP HRD), financedfrom the European Social Fund and by the Romanian Govern-ment under the projects POSDRU/89/1.5/S/59323, POSDRU/88/1.5/S/59321, POSDRU/107/1.5/S/76945 and POSDRU/6/1.5/S/6.

References

[1] D. Kragic, H. I. Christensen, Advances in Robot Vision, Robotics andAutonomous Systems 52 (2005) 1–3.

[2] D. Kim, R. Lovelett, A. Behal, An Empirical Study with Simulated ADLTasks using Vision-Guided Assistive Robot Arm, in: Proc. of the IEEE11th Int. Conf. on Rehabilitation Robotics ICORR 2009, Kyoto, Japan,2009.

[3] R. Hartley, A. Zisserman, Multiple View Geometry in Computer Vision,Cambridge University Press, 2004.

[4] R. Bajcsy, Active Perception, Proc. Of the IEEE 76 (8) (1988) 996–1005.[5] F. Chaumette, S. Hutchinson, Visual Servo Control Part I: Basic Ap-

proaches, IEEE Robotics and Automation Magazine 13 (4) (2006) 82–90.[6] M. Mirmehdi, P. Palmer, J. Kittler, H. Dabis, Feedback Control Strategies

for Object Recognition, IEEE Trans. on Image Processing 8 (4) (1999)1084–1101.

[7] Q. Zhou, L. Ma, D. Chelberg, Adaptive Object Detection and RecognitionBased on a Feedback Strategy, Image and Vision Computing 24 (2006)80–93.

[8] J. Peng, B. Bahnu, Closed-loop Object Recognition Using Reinforce-ment Learning, IEEE Trans. on Pattern Analysis and Machine Intelli-gence 20 (2) (1998) 139–154.

[9] J. Marchant, C. Onyango, Model-based Control of Image Acquisition,Image and Vision Computing 21 (2003) 161–170.

[10] S. Gutierrez, J. Marroquin, Robust Approach for Disparity Estimation inStereo Vision, Image and Vision Computing 22 (2004) 183–195.

[11] D. Ristic, Feedback Control in Image Processing, Shaker Verlag, Aachen,Germany, 2007.

[12] S. M. Grigorescu, D. Ristic-Durrant, A. Graeser, ROVIS: RObust ma-chine VIsion for Service robotic system FRIEND, in: Proc. of the 2009Int. Conf. on Intelligent RObots and Systems, St. Louis, USA, 2009.

[13] S. Grigorescu, S. Natarajan, D. Mronga, A. Graeser, Robust FeatureExtraction for 3D Reconstruction of Boundary Segmented Objects in aRobotic Library Scenario, in: Proc. Of the 2010 IEEE/RSJ Int. Conf. onIntelligent Robots and Systems, Taipei, Taiwan, 2010.

[14] S. M. Grigorescu, Robust Machine Vision for Service Robotics, Ph.D.thesis, Bremen University, Institute of Automation, Bremen, Germany(Jun. 2010).

[15] K. Ariyur, M. Krstic, Real-Time Optimization by Extremum SeekingControl, John Wiley and Sons, New York, USA, 2003.

[16] J. Neira, A. Davison, J. Leonard, Special Issue on Visual SLAM, IEEETrans. on Robotics 24 (5) (2008) 929–931.

[17] L. Paz, P. Pinies, J. Tardos, J. Neira, Large-Scale 6-DOF SLAM WithStereo-in-Hand, IEEE Trans. on Robotics 24 (5) (2008) 946–957.

[18] T. Lemaire, C. Berger, I. Jung, S. Lacroix, Vision-Based SLAM: Stereoand Monocular Approaches, Int. Journal of Computer Vision 74 (3)(2007) 343–364.

[19] G. Welch, G. Bishop, An Introduction to the Kalman Filter, UNC-ChapelHill, 2006.

[20] M. Arulampalam, S. Maskell, N. Gordon, T. Clapp, A Tutorial on Parti-cle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking, IEEETrans. on Signal Processing 50 (2) (2002) 174–188.

[21] M. A. Lourakis, A. Argyros, SBA: A Software Package for GenericSparse Bundle Adjustment, ACM Trans. Math. Software 36 (1) (2009)1–30. doi:http://doi.acm.org/10.1145/1486525.1486527.

[22] M. Brown, D. Burschka, G. Hager, Advances in Computational Stereo,IEEE Trans. on Pattern Analysis and Machine Intelligence 25 (8) (2003)993–1008.

[23] C. Harris, M. Stephens, A Combined Corner and Edge Detection, in:Proc. of the Fourth Alvey Vision Conference, 1988, pp. 147–151.

[24] R. Gonzalez, R. Woods, Digital Image Processing, Pearson Education,3rd edition, 2007.

[25] H. Hirschmueller, Stereo Processing by Semi-Global Matching and Mu-tual Information, IEEE Trans. on Pattern Analysis and Machine Intelli-gence 30 (2) (2008) 328–341.

[26] G. Bradski, A. Kaehler, Learning OpenCV, Computer Vision with theOpenCV Library, O’Reilly Media, USA, 2008.

[27] R. Rusu, Z. Marton, N. Blodow, M. Dolha, M. Beetz, Towards 3D PointCloud based Object Maps for Household Environments, Robotics and Au-tonomous Systems 56 (11) (2008) 927–941.

[28] P. Malbezin, W. Piekarski, B. Thomas, Measuring ARToolKit Accuracyin Long Distance Tracking Experiments, in: 1st Int. Augmented RealityToolkit Workshop, Darmstadt, Germany, 2002.

[29] N. Ogren, I. Egerstedt, X. Hu, Reactive Mobile Manipulation Using Dy-namic Trajectory Tracking, in: IEEE Inter. Conf. on Robotics and Au-tomation ICRA 2000, San Francisco CA, USA, pp. 3473–3478.