Visual servoing to an arbitrary pose with respect to an object given a single known length

Abstract

A method is presented to attach reference frames to piecewise planar objects in view a single camera. This method uses Euclidean homography relationships and a single known geometric length on a single object. By attaching reference frames to objects in the scene, the method is useful in position-based visual servo control, where it allows control of pose with respect to an object. The method is distinguished from methods that require a detailed model of the object/scene to give camera pose relative to an object, and it is distinguished from methods that can only give current camera pose with respect to a pose where a reference image was taken. Simulations of the method for camera-in-hand and camera-to-hand visual servo control tasks are presented. Experiments are presented where the reconstruction method is used to estimate the pose of a vehicle. These experiments represent the initial steps in a vision-based vehicle following controller.

Full text

Page 1

Visual Servoing to an Arbitrary Pose with Respect to an Object Given a
Single Known Length1
N. R. Gans, A. P. Dani and W. E. Dixon
Department of Mechanical Aerospace Engineering
University of Florida, Gainesville, FL USA
ngans@ufl.edu, ashwin31@ufl.edu, wdixon@ufl.edu
Abstract—A method is presented to attach reference frames
to piecewise planar objects in view a single camera. This method
uses Euclidean homography relationships and a single known
geometric length on a single object. By attaching reference
frames to objects in the scene, the method is useful in position-
based visual servo control, where it allows control of pose
with respect to an object. The method is distinguished from
methods that require a detailed model of the object/scene to give
camera pose relative to an object, and it is distinguished from
methods that can only give current camera pose with respect to
a pose where a reference image was taken. Simulations of the
method for camera-in-hand and camera-to-hand visual servo
control tasks are presented. Experiments are presented where
the reconstruction method is used to estimate the pose of a
vehicle. These experiments represent the initial steps in a vision-
based vehicle following controller.
I. INTRODUCTION
Position-based visual servo (PBVS) control methods [1]–
[4] use three dimensional scene information that is recon-
structed from image information. That is, the camera acts
as a “Cartesian sensor”, where pose estimation algorithms
use camera data to generate an error signal in Cartesian
space. This error signal is then used in a feedback control
law. Reconstruction methods vary in terms of computational
complexity, accuracy, and the amount of information re-
quired. Methods exist to determine the camera’s pose (i.e.
the camera’s position and orientation) with respect to an
object [5], [6]; however, these methods require accurate
geometric knowledge of the object, i.e. knowledge of the
relative coordinates of all 3D points, lines, etc. used in the
algorithm. These methods might be called “hard PBVS”
methods.
An accurate model is not available for unknown scenes and
may be hard to obtain, even for a known object. Alternately,
given two images it is possible to use the Essential Matrix
or Homography matrix [7], [8] to determine the rotation and
translation between the associated camera poses. Essential or
Homography Matrices have been used in numerous control
designs [9]–[11]. Typically, these methods could only move
to a goal pose where an image had been captured, and
are often referred to as teach by showing methods. Since
1This research is supported in part by the NSF CAREER award number
0547448, AFOSR contract number F49620-03-1-0170, by research grant
No. US-3715-05 from BARD, the United States - Israel Binational Agri-
cultural Research and Development Fund, and the Department of Energy,
grant number DE-FG04-86NE37967 as part of the DOE University Research
Program in Robotics (URPR).
Image-Based Visual Servoing (IBVS) [2], [12] methods are
typically teach by showing, these PBVS methods have some
commonality with IBVS and could be called “soft PBVS”
methods. The major drawback of these methods is that they
can only regulate the pose of the camera with respect to
a reference pose where some reference image was taken.
They are not suitable to regulate the pose of the camera
with respect to a viewed object or the pose of one viewed
object with respect to another.
This paper presents a new method to attach a reference
frame to a planar surface containing feature points. This
method uses the Euclidean Homography derived from two
images and requires knowledge of a single geometric length
between two visible points on the plane. Alternately, if a
length on the object is unknown, but the robot is well
calibrated (i.e. accurate measurements of the robot pose and
velocity are available), it is possible to attach the reference
frame using a known camera motion rather than a known
length.
The attached reference frame allows feedback control to
move the camera to an arbitrary position relative to the
object. This has numerous applications such as manipulation
and docking. Furthermore, without any further geometric
knowledge, it is possible to attach a reference frame to every
other static planar surface. There is no requirement that the
scene remain static after attaching the reference frames, so it
becomes possible to track or control moving objects after a
brief learning phase. The presented method has advantages
over both the “hard” and “soft” PBVS methods. Knowledge
of a single geometric length is much less restrictive than
the complete geometric knowledge of all lengths needed for
“hard” pose recovery methods such as [5], [6], and the ability
to achieve a pose with respect to an object rather than a
reference pose is more useful for some tasks than “soft”
methods.
Silveira, et al. [13] recently explored the ability to visual
servo a camera to an arbitrary pose. This method also
makes use of the Euclidean Homography and is capable
of positioning the camera outside its initial field of view.
However, there are major differences between the approach
in [13] and the approach presented in this paper. The method
by Silveira, et al. defines the arbitrary pose relative to a
reference pose where a reference image was taken. The
method presented here allows the camera to move to an
arbitrary pose relative to a visible object, rather than a
2008 American Control Conference
Westin Seattle Hotel, Seattle, Washington, USA
June 11-13, 2008
WeB17.2
978-1-4244-2079-7/08/$25.00 ©2008 AACC.1261

Page 2

reference pose. In order to achieve an arbitrary pose with
respect to an object, the method by Silveira, et al. requires
a priori knowledge of the relative transformation between
the reference pose and the object. Furthermore, the method
presented by Silveira, et al. decomposes the Homography
to recover rotation and translation information, while the
method presented in this paper uses the Homography matrix
only to retrieve the normal to the plane. The normal vector is
used in conjunction with a known geometric length to attach
a reference frame to the plane.
The method presented in the paper is similar in spirit to
[14], where an image-based method was presented to align a
camera perpendicular to a planar surface. Given a calibrated
robot, odometry information can generate an estimate of
object depth and achieve a desired pose with respect to the
object.
In addition, the method presented in this paper can provide
priori information required by some vision-based estimation
algorithms. For instance, [15] presents a homography-based
velocity estimator, which requires knowledge of a geometric
length and knowledge of the pose of the object in the initial
image frame. Using the reconstruction method we introduce
here, all the necessary information is recovered from just the
single geometric length. A variation of this method is used in
[16] to estimate the position of feature points if they become
occluded or temporarily left the field of view.
The paper is organized as follows. Section II, presents
background information introduces notation. The reconstruc-
tion method for the baseline case of a single set of coplanar
feature points is presented in Section III. Section IV extends
the developed method to the case of piecewise planar objects
or piecewise planar scenes. Experimental and simulated re-
sults are presented in Section V. Finally, Section VI provides
a discussion of several applications and extensions of this
technique that we are actively pursuing.
II. BACKGROUND
Consider a camera with reference frame F∗
views a collection of k ≥ 4 feature points lying in a plane
πsin front of the camera. These points have in the ordinates
¯ m, ¯ m∗
c. The camera
j∈ R3in the camera reference frame given as
¯ m∗
j= [x∗
j,y∗
j,z∗
j]T, ∀j ∈ {1...k}.
An image of the points is captured, resulting in a projection
to a set of points in the image plane πi. These image points
are given by the normalized coordinates m∗
j∈ R3given as
m∗
j= [x∗
j
z∗
j
,y∗
z∗
j
j
,1]T, ∀j ∈ {1...k}.
The plane πs has normal vector −n∗∈ R3in F∗
πi has a normal coincident with the −z-axis of F∗
constant, scalar distance
c, while
c. The
¯ s1= k¯ m∗
1− ¯ m∗
2k
(1)
is assumed to be known. These coordinate frame relation-
ships are illustrated in Fig. 1.
By moving the camera (similarly, moving πs) by a transla-
tion x(t) ∈ R3and rotation R(t) ∈ SO(3), the camera will
obtain a new pose Fc(t). The points have Euclidean and
normalized coordinates ¯ mj(t), mj(t) ∈ R3in Fc(t) given
by
¯ mj(t)=[xj(t),yj(t),zj(t)]T, ∀j ∈ {1...k}
[xj(t)
zj(t),1]T, ∀j ∈ {1...k}.mj(t)=
zj(t),yj(t)
A homography exists that maps m∗
raphy can be defined as H(t) ∈ R3×3such that
jto mj(t). This homog-
mj
=
z∗
zjHm∗
³
j
j
(2)
mj
=αR +x
d∗n∗T´
m∗
j,
(3)
where αj =
is decomposed to recover R(t),
...k} [7], [17]. Note that translation is only recovered up to
a scaled factor xd(t) =
unknown.
Using standard projective geometry, the Euclidean coor-
dinate ¯ mj(t) is expressed in image-space pixel coordinates
as pj(t) = [uj(t),vj(t),1]T. The relationship between pj(t)
and mj(t) is given by
z∗
j
zjis a scalar depth ratio. The matrix H(t)
x(t)
d∗, n∗and αj, ∀j ∈ {1
x(t)
d∗ , and the depth d∗is generally
pj= Amj,
(4)
where A ∈ R3×3is a constant, invertible, upper-triangular
camera calibration matrix [8]. Using (4), the Euclidean
relationship in (3) is expressed as
pj
=αjAHA−1p∗
αjGp∗
j.
j
=
(5)
Given knowledge of A and k ≥ 4, it is possible to solve a
set linear equations for G(t) and recover H(t), R(t),
n∗and αj(t).
x(t)
d∗,
III. GEOMETRIC RECONSTRUCTION OF COPLANAR
FEATURE POINTS
A. Attaching a Reference Frame to a Planar Object
The Euclidean homography algorithm is sufficient to es-
timate the rotation and scaled translation of the camera or
object with respect to some reference pose. However, it is not
sufficient to solve for the pose of the camera with respect to a
viewed object or the pose of one viewed object with respect
to another. The following development provides a method
where the Euclidian homography algorithm is used along
with some additional information to attach a reference frame
Fsto a planar object, with rotation and translation given with
respect to the camera frame Fc. That is, the development in
this section is used to determine the rotation and translation
between Fsand Fc.
Consider a camera viewing a planar object with four or
more distinguishable feature points, and denote the feature
point plane as πs. If the camera and/or object move over
1262

Page 3

Fig. 1.Elements of the reconstruction method
time, a set of linear equations can be solved for the Homog-
raphy matrix H(t) as in (2). The matrix H(t) is decomposed
into R(t),x(t)
F∗
vector of the planar object in the current frame Fcis given
as
n(t) = R(t)n∗.
d∗ , and n∗as in (2), where the reference frame
ccan be taken as Fc(t0), i.e., the initial frame. The normal
The goal is to attach a reference frame Fs(t) to the planar
object. Without loss of generality, the origin of Fs(t) is
assigned to the point ¯ m1. The orthonormal vectors ix, iy,
iz∈ R3that define Fs(t) form a rotation matrix R(t) in Fc
as
R =£
The columns of R(t) in (6) are defined as
ix
iy
iz
¤.
(6)
iz
=−n
¯ m2− ¯ m1
¯ s1
−n ׯ m2− ¯ m1
(7)
ix
=
(8)
iy
=
¯ s1
,
(9)
where the constant distance between the two feature points
¯ s1 = k¯ m1− ¯ m2k is assumed to be known. If ¯ m1(t) and
¯ m2(t) were known, then ix and iy can be determined
from (8) and (9) since ¯ s1 is assumed to be known. To
solve for ¯ m1(t) and ¯ m2(t), a new plane π0
with normal −n(t) (so π0
taining the normalized image point m1(t). A line l is
defined from the origin of Fc through m2(t) and ¯ m2(t).
The plane π0
distance between m1(t) and m0
Fig. 1.
The primitives l and π0
q ∈ R3that satisfy the implicit functions
l={q | q − um2= 0,∀u ∈ R}
π0
s
={q | n · (q − m1) = 0, q,n,m1∈ R3}. (11)
The intersection of π0
u =n · m1
n · m2.
sis defined
sis parallel to πs) and con-
sintersects l at a point m0
2. The unknown
2(t) is s1, as illustrated in
sare defined by the sets of points
(10)
sand l occurs when
(12)
The expressions in (10) and (12) are combined to solve for
the point q = m0
2=n · m1
n · m2m2.
The solution for m0
2as
m0
2(t) is used to solve for s1as
s1= km0
2− m1k
(13)
and the properties of similar triangles is used to calculate the
following:
s1
¯ s1
=km1k
k¯ m1k=
°°°m
2(t)
0
2
°°°
k¯ m2k.
°°° are now known or
(14)
Since s1, ¯ s1,km1(t)k, and
measurable, (14) is used to solve for k¯ m1(t)k and k¯ m2(t)k,
which is used to recover ¯ m1(t) and ¯ m2(t) as
°°°m
0
¯ m1=k¯ m1k
km1km1, ¯ m2=k¯ m2k
km2km2.
Solutions for ix, iy, and R(t) can now be determined
from (8), (9), and (6), respectively. Since Fs(t) is attached
to ¯ m1(t), the translation is simply given by x(t) = ¯ m1(t).
Furthermore, since ¯ m1(t) is determined, it is now possible
to solve for the distance d(t) as
d = n · ¯ m1.
(15)
If the constant length ¯ s is not known, but d(t) is known
or estimated using known camera motions, then the fact
that d(t) = zj(t)nT(t)mj(t) can be used to solve for the
Euclidean coordinates of ¯ mj(t) as
¯ mj=
dmj
n · mj, ∀j ∈ {1...N}.
(16)
Solving for the Euclidean coordinates of two points ¯ m1(t)
and ¯ m2(t) allows ¯ s1to be estimated, which can be used to
solve for the frame Fs(t).
With d determined, and mj, ∀j ∈ {1 ...k} measurable,
the 3D coordinates in Fc(t) of all points ¯ mj= [xj,yj,zj]T,
∀j ∈ {1 ...k} is determined as
¯ mj= zjmj=
d
n · mj
¯ mj,
(17)
where
d=n · ¯ mj= n · mjzj
d
n · mj.
(18)
zj
=
(19)
B. Visual Servoing to an Arbitrary Pose
A simple visual servo controller that utilizes the above
method is now presented. The control objective is to move
the camera to a goal pose by generating a continuous camera
velocity v(t) ∈ R6. The strength of our method is that the
camera is accurately regulated to a pose defined with respect
to a planar object, i.e. teach by showing is not required. Other
PBVS methods that use the Homography matrix (e.g. [9],
[18]) are restricted to positioning the camera with respect
to the camera poses F∗
1263
cor Fc. Using the method presented

Page 4

here, there is no need for an a priori goal image or reference
image. Other pose reconstruction methods to derive pose
with respect to an object may require detailed structural
information (e.g. [5], [6]).
Let Rdand xddenote the desired, constant rotation and
translation of the camera with respect to the static plane
frame Fs, such that the image points on the plane are visible
to the camera at the desired pose. The camera is at a pose F∗
that is currently unknown with respect to Fs. By performing
a small translation, the camera moves to a pose Fc(t1) and
the Homography H(t1) is computed and decomposed to
R(t1),
d∗
With n(t) = R(t)n∗known, the pose of Fs can be
determined with respect to the current camera frame Fc(t).
The rotation and translation between Fc(t) and Fs are
denoted as Rc(t) and xc(t) respectively. A constant, desired
pose with respect to Fsis denoted as Fd. The rotation error
Re(t) ∈ SO(3) and translation error xe(t) ∈ R6between
Fdand Fc(t) is given by
Re
=RdRT
xe
=−RdRT
The rotation error matrix Re(t) is locally mapped to
c
x(t1)
and n∗.
c
(20)
(21)
cxc+ xd.
Re(t) → u(t)θ(t) ∈ R3,
where θ(t) ∈ R is the rotation angle about the axis u(t) ∈
R3. The pose error vector is then defined as
e = [xT
e, uθT]T∈ R6.
(22)
The camera velocity is given by
ξ = [vT,ωT]T∈ R6,
(23)
where v(t) ∈ R3is the linear velocity and ω(t) ∈ R3is
angular velocity. The time derivative of (22) is given as a
function of camera velocity by
·e = Lξ,
(24)
where L(t) ∈ R6×6is a Jacobian-like matrix that maps
camera velocity to pose error the derivative given as
∙
In (25), Rvc(t) ∈ SO(3) is the rotation matrix from the
frame in which ξ(t) is measured to Fc(t). Rvc(t) is identity if
the camera frame and input velocity frame are the same. The
Jacobian like matrix Lω(t) ∈ R3×3maps angular velocity to
d
dt(uϕ). As shown in [9], Lω(t) is given by
Ã
where I is the 3×3 identity matrix, u×(t) ∈ R3is the skew
symmetric matrix form of the vector u(t), sinc(ϕ) =sin(ϕ)
L =
Rvc
03×3
03×3
RvcLω
¸
.
(25)
Lω= I −ϕ
2u×+1 −
sinc(ϕ)
sinc2¡ϕ
2
¢
!
u2
×,
ϕ
,
and sinc(0) = 1.
Based on (22) and (24), the linear and angular velocity
input
ξ = −λL−1e
(26)
Fig. 2.Example of Multiple Planar Surfaces
can then be defined, where λ is a scalar gain. Combining
(24) and (26) gives the closed-loop error derivative
·e = −λe.
(27)
The closed-loop system in (26) can be shown to asymptoti-
cally stabilize the error to zero, thereby bringing the camera
to the goal pose. A simulation this controller is presented in
Section V-A.
IV. GEOMETRIC RECONSTRUCTION OF PIECEWISE
PLANAR OBJECTS AND SCENES
A. Reconstructing the Geometry of Multiple Planes with
Respect to the Camera
The previous development is now extended to the case of
multiple planar patches and piecewise planar objects, given
knowledge of only a single geometric length on a single
static object in the scene. Consider a large sample of points
P visible to the camera. These points are grouped into g sets
of coplanar points Ph⊂ P, ∀h ∈ {1 ...g}, where all points
in Ph lie in a plane πh. The sets Ph may overlap, i.e., a
point may be in more than one set.
Segmenting a set of points into coplanar sets is not a
trivial task. The sets can be distinguished through human
interaction, scene knowledge (e.g., multiple light objects on
a dark background) or various automated methods [19], [20].
This example assumes that each set Phis well conditioned
in the sense that it contains no more than three collinear
points, as illustrated in Fig. 2.
In the following development, the camera is assumed to
undergo a rotation R(t) and translation x(t) from a reference
frame F∗
coordinates
cto a frame Fc(t). The points in each set Phhave
¯ m∗
hj
∀j
=[x∗
hj,y∗
{1...Nh}, ∀h ∈ {1...g}
[xhj,yhj,zhj]T,
{1...Nh}, ∀h ∈ {1...g}
c and Fc(t), respectively. These points
project to image points with normalized coordinates m∗
hj,z∗
hj]T,
∈
=
¯ mhj(t)
∀j∈
in the frames F∗
hj
1264

Page 5

and mhj(t), as described in Section II. Each set of points in
the images are related by a homography Hh(t) defined by
z∗
hj
zhj
z∗
hj
zhj
Note that R(t) and x(t) are the same for all point sets Ph,
since all coordinate changes are due to the motion of the
single camera. However, each plane πhis different; therefore,
each set of points will have different d∗
From m∗
n∗
d∗
h
subsequent development is based on the assumption that a
single geometric length between two points in a single set is
known. Without loss of generality, this length is assumed to
be known in set P1. Given this geometric length, a reference
frame Fs1 is attached to plane π1 and the development in
Section III is used to solve for Rc1(t), xc1(t), d1(t), d∗
all ¯ m1j, ∀j ∈ {1 ...N1}. The translation x(t) can then be
recovered from d∗
mhj
=Hhm∗
µ
hj
(28)
mhj
=
R(t) +x(t)
d∗
h
n∗T
h
¶
m∗
hj.
(29)
h, n∗
hand Hh(t).
hjand mhj, it is possible to recover Hh(t),
h, R(t), and xh(t) =
x(t)
for all h ∈ {1 ...g}. The
1and
1as
x(t) = d∗
1x1(t).
Given x(t), each d∗
scaled translations xh(t) as
h, ∀h ∈ {2 ...m} is recovered from the
d∗
h=xT
hx
kxhk.
Once each d∗
...Nh}, ∀h ∈ {2 ...m} can be recovered as in (16). From
knowledge of ¯ m∗
points can be estimated for each plane, and the frames Fsh
can be attached to the plane πh. Thus, (6)-(9) can be used to
solve for Rchand xch, ∀h ∈ {2 ...m}. Given the rotation
and translation from each plane πhto the reference camera
frame Fc, the rotation and translation between each planar
patch can be recovered.
For a stationary camera viewing multiple moving planar
objects, the analysis cannot be performed because there is not
a common R(t) or x(t). If a geometric length is known on
each object, then the analysis in Section III can be performed
for each moving plane.
hhas been determined, all ¯ m∗
hj, ∀j ∈ {1
hj, a constant length ¯ shbetween two feature
B. Visual Servoing a Piecewise Planar Object to an Arbi-
trary Pose with Respect to an Unknown Object
A simple visual servo controller that utilizes the devel-
opment for multiple planar objects is now presented. The
task is to move a piecewise planar object in the view of a
camera to a goal pose with respect to a second, stationary
planar object. If a single known length is available on the
object, then by temporarily keeping that object static, that
length can be used to calculate geometric knowledge of all
planar objects in the scene. Once geometric knowledge of
the scene is available, there is no need for the controlled
object to remain static, and a positioning control task can be
performed.
Assume a camera is at pose F∗
sisting of two planar objects π1and π2, where π1can accept
velocity inputs and π2 is static. Assume further that the
camera is capable of at least some small autonomous motion.
After moving to an arbitrary pose Fc(t1), the homographies
H1(t) and H2(t) are determined. With n1(t) = R1(t)n∗
n2(t) = R1(t)n∗
solved with respect to the camera frame Fc(t). The rotation
and translation from Fs1 to Fc(t), is denoted Rc1(t) and
xc1(t), respectively. Likewise, the rotation and translation
from Fs2 to Fc(t), are given by Rc2(t) and xc2(t). For
simplicity, assume that the camera does not move again after
the reference frames have been assigned to the planes.
Define a constant, desired rotation and translation Rdand
xdwith respect to Fs2. The rotation Re(t) and translation
xe(t) from the desired pose to the current camera pose Fc(t)
is given by
cand views a scene con-
1and
2known, the pose of both planar objects are
Re
xe
=(Rc2Rd)TRT
−Rd(−RT
c1
(30)
(31)
=
c1RT
c2xc2+ xc1) + xd.
The rotation matrix Re(t) is locally mapped to Re(t) →
ue(t)θe(t) ∈ R3, where θe∈ R is the angle of rotation about
the axis ue∈ R3. The pose error vector is then given by
e(t) = [xe(t), ue(t)θe(t)] ∈ R6.
Similar to the discussion in Section V-A, a velocity vector
for Fs1that will asymptotically stabilize the error in (32) is
given by
ξ = −λL−1e,
where L(t) is a matrix that maps the velocity of Fs1to the
derivative of the pose error and λ is positive scalar gain.
Simulations of such a controller are given in Section V-B.
(32)
(33)
V. EXPERIMENTAL RESULTS
A. Simulation of Eye In Hand Visual Servoing to an Arbi-
trary Pose
The control task described in Section III-B is simulated
for a set of four coplanar points. The points are configured
in a square, but only the distance between two points is
assumed to be known. There is perfect camera calibration,
and quantization noise is added by rounding the image
feature coordinates to the nearest integer. The desired
camera pose is [xT
respect to a frame Fs attached to one of the feature
points. The initial pose error is [xT
[0.6721, 0.7489, 0.3471, 0.4092, −0.5855, −0.1970],
and thedevelopedmethod
tialposeerrortobe
[0.77610, 0.7378, 0.3173, 0.4086, −0.5846, −0.1967].
Visual servoing is performed to regulate the pose error. The
pose error over time is given in Fig. 4. The trajectory of the
feature points in the image is given in Fig. 3.
d,uT
dθd] = [0,0,−1.2,0,0,π]Twith
e(0),uT
e(0)θe(0)] =
estimates
e(0),uT
theini-
=
[xT
e(0)θe(0)]
B. Simulation of Vehicle Regulation with Respect to an
Unknown Object
A simulation of the task given in Section IV-B is presented
in this section. The objective is to move a controllable planar
1265