IEEE Robotics & Automation Magazine - December 2013 - 24

safety volume around the vehicle or when the target is in the
field of view of the vehicle. In particular, this force depends on
the distance between the UAV and the obstacles/target.
Pose Estimation
The proposed approach requires the pose estimation of the
UAV flying in a cluttered unknown environment. Several
approaches have been developed to estimate the UAV pose
using one onboard camera. In [18] and [19], the pose is estimated by fusing data extracted from a camera flow and IMU
measurements. A square root unscented Kalman filter is used
in [20] to solve the simultaneous localization and mapping
(SLAM) problem for a single camera. In [21], a real-time
monocular SLAM is implemented that uses an extended
Kalman filter and a straight-line detector.
In the proposed scheme, the odometry of the UAV is evaluated on the basis of an onboard monocamera video streaming, which is the input to a module based on PTAM [13],
originally developed as a camera tracking system for augmented reality. It requires no markers, premade maps, known
templates, or inertial sensors. The low-level egomotion estimation provided by PTAM and the available IMU measurements have been suitably combined with an extended
Kalman filter to develop an effective odometry estimation
module. Figure 2 illustrates the odometry of a test path, where
the initial and the final pose of the vehicle are coincident. The
total length of the considered path is 11.3 m, while the norm
of the residual positional error is 2.8 cm.
In the indoor experiments, as described in the
"Experiments" section, an external tracker has been used to
enhance the stabilization of the pose of the UAV.
3-D Environment Map
The visual measurements together with the vehicle odometry
are employed for the online estimation of a 3-D occupancy
2

x, y, z (m)

1.5
1
0.5
0
-0.5
(a)

t (s)
(b)

Figure 2. The vehicle trajectory estimated by the proposed PTAMbased module of a spiral-like path of 11.3-m length (the initial pose
is coincident with the final pose). (a) A 3-D representation of the
estimated vehicle odometry. (b) The estimated positional trajectory
(x-component in red, y-component in green, and z-component in
blue) with respect to the camera frame, which results in a norm of
the final residual positional error of 2.8 cm.

IEEE ROBOTICS & AUTOMATION MAGAZINE

DECEMBER 2013

map of the surrounding environment. Simultaneously, an
image-target identification algorithm runs to localize the target,
which could be present in the observed scene, in the map. In
the following subsections, the employed camera model and
obstacle localization algorithm, as well as the 3-D occupancy
map construction procedure, are presented.
Camera Model
In this article, the well-known pinhole camera model is
employed [22]. The perspective relation between the 3-D coordinate of a point p = [ p x p y p z] T expressed in the world reference frame and its 2-D projection on the image plane of the
camera, expressed in pixel coordinate, is given by
Xf

>Y f H = X 6I 3
1

0@ T cb up,

where m > 0 is a projection scale factor, I is the identity
matrix, up = 6 p T 1@T is the homogeneous representation of
p, and X is the camera calibration matrix, i.e., the camera
intrinsic parameters, given by
fa x 0 X 0
X = > 0 fa y Y0 H,
0 0 1
where fa x and fa y are the row and column focal length,
respectively, and 6X 0 Y0@T is the position of the image center
expressed in pixels. The matrix T cb represents the homogeneous transformation between the camera frame and the base
frame T cb = T cv (T bv) -1, where T cv is the constant homogeneous transformation matrix between the camera frame and
the UAV body frame, i.e., the camera extrinsic parameters,
with the origin in the center of mass of the vehicle, and T bv is
the current homogeneous transformation matrix between the
body frame and the base frame, provided by the odometry
estimation algorithm. More details can be found in [22].
Obstacle Localization
In the proposed approach, the estimation of the positions of
the obstacles present in the observed scene is evaluated using
information extracted by a single camera and the odometry.
In detail, since only one camera is available, while at least two
different views of an image feature are required, the image
flow is continuously stored in a buffer. First, the grabbed
images are synchronized temporally with the pose estimation
of the vehicle, provided by the odometry estimation system.
Then, only the most robust keypoints of the image are
extracted and linked to the corresponding image-odometry
pair. For this purpose, the well-known Shi-Tomasi corner
detector [23] is employed, even if other keypoints extractors
could also be used as well. Finally, this set of information, i.e.,
the key-frames, is stored in a finite-length buffer.
Hence, an image elaboration algorithm selects a pair of keyframes, grabbed from positions a sufficient distance from each

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - December 2013