3.1 Introduction

This chapter discusses recent advances in the use of computer vision for the control of micro aerial vehicles (MAVs). The term MAV typically denotes a class of small-scale unmanned aerial vehicles like multirotor helicopters, for example, a quadrotor helicopter. Figure 3.1 shows such an MAV equipped with digital cameras for control and 3D mapping.

MAVs have a big potential to be used in a various number of applications, for example, search–and-rescue scenario, surveillance, industrial inspection, delivery services, and so on. MAVs can carry a variety of sensors; however, the small scale imposes a strict weight limit. Almost all types of MAVs, however, are able to carry a digital camera and this makes it possible for them to take aerial images or, in general, images from vantage points not reachable otherwise. Already this capability enables an immense variety of applications. Piloting an MAV, however, takes a lot of training and requires constant attention of the pilot. It also requires that the MAV operates within the line of sight. These limitations can be overcome by an autopilot system with the main task of keeping the MAV hovering on a spot. In outdoor environments, this can be achieved using GPS; however, a MAV operating indoors cannot rely on GPS and needs an alternative sensor for the autopilot system. Lately, onboard cameras have been successfully used within such an autopilot system. Computer vision algorithms compute the ego-motion of the MAV from camera images, in most cases then these measurements are fused with measurements of inertial measurement units and used in a control loop to hover the MAV. To achieve these results, some serious challenges have had to be overcome:

• Limited onboard processing power for image processing;
• High frame rate for control is necessary;
• High reliability;
• Ego-motion estimation from a single camera cannot measure metric scale.

A main insight has been that fusing inertial measurement unit (IMU) and camera-based measurements allows for robust and efficient ego-motion estimation algorithms.

In addition to ego-motion estimation, the camera images can also be used for environment sensing and interpretation. From camera images, it is possible to compute a 3D map of the environment that can be used for autonomous navigation and exploration. Having the ability to map the environment in 3D allows collision-free navigation and is a prerequisite for autonomous operation of the MAV. In the following, computer vision methods for MAV control, 3D mapping, autonomous navigation, and scene interpretation are introduced and discussed.

In a number of publications, the astounding flight performances of MAVs have been demonstrated, ranging from acrobatics to fast and dynamic maneuvers (see Mellinger et al. (2011 2010), Michael et al. (2010a), Mueller et al. (2011), and Schoellig et al. (2010)). However, until now, these impressive performances could only be performed within a specially instrumented area (see Lupashin et al. (2011) and Michael et al. (2010b)) where a tracking system (typically cameras observing markers on the MAVs) computes the exact position and orientation of the MAV from outside. Systems with onboard sensors are far from reaching these performance parameters. Naturally, MAV control using onboard sensors is a heavily researched area, with the goal to achieve the aforementioned capabilities with onboard sensors. This research not only is limited to digital cameras as onboard sensors but also includes the use of laser range-finder, depth cameras, or combinations of different sensors (see Achtelik et al. (2009 2011), Ahrens et al. (2009), Bachrach et al. (2009), Bills et al. (2011a), Bloesch et al. (2010), Eberli et al. (2011), Engel et al. (2014), Forster et al. (2014b), Grzonka et al. (2012), Herisse et al. (2008), Hrabar et al. (2005), Klose et al. (2010), Loianno and Kumar (2014), Nieuwenhuisen et al. (2015), Shen et al. (2011 2012 2013a), Yang et al. (2014), Zingg et al. (2010), and Zufferey and Floreano (2006)).

3.2 System and Sensors

This section gives a brief overview of system design and sensors of a MAV. The system design described is from the Pixhawk MAV platform (see Meier et al. (2012)), but it is exemplary for many other MAV platforms, for example, see Achtelik et al. (2011), Scaramuzza et al. (2014), and Schmid et al. (2014). The system design is illustrated in Figure 3.2. A main characteristic is the existence of a low-level processing unit for flight control and a high-level processing unit for image processing and high-level tasks (e.g., path planning). The main task of the low-level processing unit is state estimation and control. State estimation has to run with high update rates and takes the measurements of IMU, digital compass, and visual pose as input. An attitude controller and pose controller use these measurements to steer the motors. Stateestimation and control have to run in real time; thus, it is implemented directly on a microcontroller. Detailed information about the low-level control process can be found in Bouabdallah et al. (2004) and Mahony et al. (2012).

Image processing and high-level tasks, however, need a powerful onboard computer; thus, the MAV is also equipped with a standard, but small scale, Linux computer. These computers run image processing, visual localization, obstacle detection, mapping, and path planning onboard the MAV.

The visual localization module computes the full 6-DOF pose of the MAV. The visual pose is fed to the state estimator for position control. The stereo processing module computes real-time disparity maps from the front-looking stereo pair, which are used by the visual localization module as well as the mapping module.

The minimal sensor set of a MAV comprises an IMU consisting of three-axis accelerometers and three-axis gyroscopes. This IMU can track the attitude of the MAV that is used for attitude control. A digital compass and a barometric pressure sensor are typically used to keep a constant heading and a constant height. For position control that is necessary for automatic hovering and any kind of automatic navigation, a sensor capable of full 6-DOF pose measurements is necessary, which in this case is solved using digital cameras.

3.3 Ego-Motion Estimation

In most approaches, ego-motion estimation is achieved by fusing information from an IMU with visual measurements similar to the work in Scaramuzza et al. (2014). By means of sensor fusion, the pose of the MAV can be computed reliably at high update rates. Visual measurements are essential in this process, as the integration of inertial measurements would accumulate large drifts otherwise.

3.3.1 State Estimation Using Inertial and Vision Measurements

State estimation is a filtering approach using an extended Kalman filter (EKF) for fusing inertial and vision measurements. The inputs for the state estimator are rotational velocities and accelerations from gyroscopes and accelerometers of the IMU at high update rates and camera poses at low update rates. Figure 3.3 illustrates the state estimation process. Camera poses are represented by a 6-DOF transformation consisting of three rotational parameters and three translational parameters and need to include the uncertainty of the measurement. Depending on the visual pose estimator (monocular, stereo vision), the camera poses could include an unknown scale factor. The IMU measurements consist of three parameters for the rotational velocities and three parameters for the accelerations. The state of the EKF contains the vehicle pose, the scale factor, and biases of the IMU. For state estimation, the vehicle state is propagated forward in time using the high-rate IMU measurements. When a vision measurement becomes available (at low update rates), an EKF update step is performed using the vision pose.

3.3.1.1 Sensor Model

The vehicle pose is represented by the pose of the IMU within the world coordinate frame. The camera sensor is the offset of the IMU coordinate frame by a rotation and translation. This transformation can be computed by a calibration step or it can be included in the state estimation. Figure 3.4 illustrates the different coordinate systems. The IMU measures the angular velocity around each of the three-axis and the acceleration in each of the three-axis . These measurements are perturbed by noise and contain a bias. The real angular velocity of the system is denoted by , and the real acceleration by . The quantities are modeled by

3.1

3.2

and are the biases for the angular velocity and the accelerations, respectively. and are the noise parameters modeled as additive white Gaussian noise. The biases are nonstatic and are included in the state to be estimated. The noise parameters typically can be taken from the sensor data sheet.

3.3.1.2 State Representation and EKF Filtering

The vehicle state consists of the following necessary parameters:

3.3

• : This parameter represents the vehicle's position (i.e., IMU position) in the world coordinate system. It consists of three parameters ( position).
• : This parameter represents the vehicles' velocity. It consists of three parameters, that is, the speeds of all the three axes.
• : This parameter represents the vehicles' orientation within the world coordinate system described by a rotation. The world coordinate system typically is chosen to be aligned with the gravity vector.
• : This parameter is the bias for the gyroscopes and is represented as a three-vector.
• : This parameter is the bias for the accelerometers and is represented as a three-vector.
• : This parameter represents the scale factor between the IMU and the camera measurements. If the camera measurements are already in metric scale (e.g., when using a stereo system), then this parameter will converge to 1 (or can be omitted from the state). However, when using a monocular camera system, ego-motion can only be estimated up to an unknown scale factor, which is then represented by .
• : This parameter represents the translational offset between camera sensor and IMU. When computed by a pre-calibration step, this parameter can be omitted from the state vector.
• : This parameter represents the rotational offset between camera sensor and IMU. When computed by a pre-calibration step, this parameter can be omitted from the state vector.

The state transition model of the EKF is used without control input and is defined as

3.4

The observation model is defined as

3.5

The current vehicle state and its uncertainties will be propagated forward in time using inertial measurements of high update rate. On availability of a vision measurement, the vehicle state and its uncertainties will be updated.

The state propagation needs two steps:

1. 1. Propagate state variables .
2. 2. Propagate the state covariance matrix .

is the Jacobian of the state transition function and is the Jacobian of the observation function.

The state transition function is explained as follows. The new position is calculated by adding the doubly integrated accelerations to the current position, and the new orientation is calculated by adding the integrated rotational velocities to the current orientation. The biases, scale, and calibration parameters are kept constant in the prediction step. For detailed derivation of and , the reader is referred to Weiss and Siegwart (2011).

On the availability of a visual pose estimate with position and orientation and its covariances, the filter update consisting of two steps can be carried out:

1. 1. Update state estimate
2. 2. Update covariance estimate.

For the detailed update equations, the reader is referred to Weiss and Siegwart (2011).

Please note that the observation process assumes that the vision system is able to directly observe the 6-DOF full pose. In addition, this means that any computer vision algorithm that delivers a pose estimate plus its uncertainties can be used for the filtering process. In the following sections, three different vision algorithms will be explained that already have been used successfully for the control of a MAV.

3.3.3 MAV Pose from Stereo Vision

The big advantage of stereo vision is that the metric scale of camera poses and 3D points can be computed directly. This means that scale drift as present in a monocular system cannot happen. And in particular for the case of state estimation, it makes the inclusion of a scale factor in the MAV state unnecessary. Furthermore, the triangulation of 3D points always happens from two precisely pre-calibrated image frames. Camera pose from stereo vision can be computed either from 3D to 3D correspondences or 3D to 2D correspondences. The 3D–2D methods recently gained a lot of popularity and is therefore described here. For the 3D–3D method, the reader is referred to Scaramuzza and Fraundorfer (2011).

The basic outline of the 3D–2D method is to use the PnP algorithm to compute camera pose from 3D to 2D correspondences. 3D points are triangulated from each stereo image pair. To compute the current pose, feature matching is performed between one image of the current stereo image pair and one image of the previous stereo image pair. The PnP algorithm is then used to compute the camera pose from corresponding 2D feature points of the current stereo image pair and 3D points from the previous stereo image pair (Figures 3.5 and 3.6).

1. 1. Capture a stereo image pair , .
2. 2. Extract, match, and triangulate features between , .
3. 3. Capture new stereo image pair , .
4. 4. Extract features and match between and .
5. 5. Compute camera pose from 3D to 2D matches using the PnP algorithm and random sample consensus (RANSAC) as a robust estimator.
6. 6. Refine camera pose and 3D points with bundle adjustment.
7. 7. Triangulate all new feature matches from stereo image pair , .
8. 8. Repeat at step 3.

Stereo vision processing adds additional computational complexity to the pose estimation process. It is necessary to create matches between the left and right images of the stereo pair and between the current and previous image pair. However, feature matching between the left and right images can be done very efficiently as the camera poses are fixed and pre-calibrated. The corresponding feature needs to lie on the epipolar line and the search for a feature correspondence can be restricted to this epipolar line. Stereo processing, however, has been shown to run in real time on dedicated hardware (e.g., FPGAs; see Schmid et al. (2014)), freeing the main computer from the low-level vision tasks.

The described method is not limited to stereo systems but can be utilized for any RGBD sensor system, that is, a sensor that captures depth (D) and a camera image (RGB). A popular example for such a sensor is the Kinect sensor. Currently, a wide variety of RGBD sensors are developed, and all of these sensors will work as well with the above-described algorithm.

3.3.4 MAV Pose from Optical Flow Measurements

The use of optical flow for controlling MAVs has been popularized with the appearance of Parrot's AR Drone. The AR Drone comes with a position control mechanism based on optical flow (see Bristeau et al. (2011)), which allows it to hover stable on the spot when there is no user input. The main benefits of using the optical flow algorithm for pose estimation are the possibility to achieve very high update rates (e.g., >100 Hz) and that it can easily be implemented on resource-limited embedded controllers. However, the algorithm comes with some limiting assumptions. The optical flow algorithm measures pixel-shift in the image plane. If the image plane is, for example, parallel to the ground plane, the observed pixel-shift is proportional to the camera motion. If the distance between camera and ground plane gets measured (e.g., by an ultrasound sensor), the pixel-shift can be translated into metric camera movements. To disambiguate optical flow that gets introduced from rotational motion and translational motion, IMU measurements can be used to correct for the rotational part. Necessary components for such a metric optical flow sensor are a camera sensor, a distance sensor (e.g., ultrasound or infrared) and a three-axis gyroscope. The images for the camera sensors are used to compute and pixel-shifts using the optical flow algorithm. The distance sensor is used to measure the distance between the camera and the world plane that induces the optical flow to translate the pixel-shifts into metric shifts. Furthermore, the distance sensor is used to track changes in the -direction (height). The gyroscope is used to measure rotational velocity of the sensor between two image frames. In this way, the rotationally induced part of the optical flow can be computed and subtracted from the measured optical flow. The camera movement (translation) can be computed from the remaining metric optical flow and distance measurements in metric units. Themathematical relation is given in Eq. (3.6). The metric translation of the camera (i.e., the translation of the MAV) is denoted by , where the movements are in , and directions. Eq. (3.6) gives the relation of how to compute from the measured optical flow between two images , the measured rotation between two images and the measured depths. The rotation between two images is measured by the gyroscopes and denoted by the rotation matrix . is the measured optical flow between the two images in pixels. is the image coordinate of the location in image for which the optical flow has been computed. are the distance measurements in meters to the ground plane for the images , respectively, and is the focal length of the camera in pixels.

3.6

Equation (3.6) can be derived from the simple equation for optical flow given by Eq. (3.7) and substituting the quantities and with the terms of Eqs (3.8) and (3.9). Equations (3.8) and (3.9) describe the camera projection of a 3D point before and after the movement of the camera with rotation and translation .

3.7

3.8

3.9

The geometric relations and quantities used in the derivation are illustrated in Figure 3.7.

Equation (3.6) describes how to compute for one image location and the optical flow. In practice, multiple flow vectors within an image are computed (e.g., by KLT feature tracking), which gives rise to multiple estimates of , which can be averages for robustness. The algorithm can now be summarized as follows:

1. 1. Compute optical flow between consecutive images using the KLT method (see Shi and Tomasi (1994)).
2. 2. Measure distances with distance sensor.
3. 3. Measure rotation matrix with gyroscopes.
4. 4. Compute using Eq. (3.6).

To achieve the high update rates needed for MAV control, two suggestions can be followed. First, by using a low-resolution camera (e.g., ), a high image capture rate can be achieved. This could, for instance, be done by turning on hardware binning on a standard VGA camera, which will reduce the resolution but increase the image capture rate. Together with a low resolution, a zoom lens is suggested such that small details on the ground plane can be resolved for better feature tracking. Second, by computing KLT feature tracks only for a fixed number of image locations defined on a grid, the computations for the feature detection part of KLT can be avoided and the number of computations will stay constant.

These ideas are actually implemented in the PX4Flow sensor depicted in Figure 3.8. The PX4Flow sensor computes metric optical flow as described earlier. For this, the sensor is equipped with a camera, gyroscopes, an ultrasound ranging device, and an ARM processor that performs the optical flow computation. The PX4Flow sensor is described in detail in Honegger et al. (2013).

3.4 3D Mapping

3D mapping, that is, the generation of 3D data from images, is one of the most prominent tasks for MAVs. MAVs provide unique vantage points for digital cameras as compared to street-level images and traditional aerial images. For building reconstruction in urban scenarios, images can be taken from very close to the building. Images can be taken facing straight down (called nadir images), looking at the facades directly or from general oblique directions at low heights. This however results in nonuniform flight patterns in contrast to the typical regularly sampled grids used in aerial imaging. Computer vision systems that generate 3D data from image sets are known under the term SfM systems. These systems can also be used to process image data taken by MAVs. However, by working only on the image data, these systems neglect the additional information provided by the sensors onboard of the MAV that are used for MAV control. These sensors can be used to improve quality and efficiency of SfM systems in a number of ways.

Figure 3.9 shows the basic building steps that are common to most SfM systems. Depicted by the arrows from the right side are the additional sensor measurements available from the MAV that can be used to improve the individual steps.

In the feature extraction step, a lot of computational power is used to extract viewpoint, scale, and orientation-invariant features and descriptors. Such an invariance, however, can also be achieved by using IMU measurements from the MAV. For the case of a downward-looking camera, the viewpoint invariance can be achieved by aligning all images with the earth's gravity normal. The deviation of the MAV from this normal is measured by the MAV's IMU. The images can be pre-rotated such that all the image planes are parallel, thus removing viewpoint changes. The remaining rotation invariance to the yaw angle can be achieved by utilizing the MAV's absolute compass measurements and by applying an in-plane rotation. Finally, scale invariance can be achieved by measuring the height over ground with the MAV's pressure sensor and correcting for this scale change. After this pre-processing, simpler, less complex features and descriptors can be used. The extraction process is faster and the matching ambiguity gets typically reduced (see Meier et al. (2012)).

In the image matching step, traditionally an exhaustive pairwise matching is computed with the main goal to establish an ordering of how to process the images. This step is extremely time consuming; it consists of descriptor matching and two-view geometry estimation for match verification. Here, additional measures from the MAV's sensor can speed up the process significantly and also increase robustness. The GPS positions of the MAV can be used to avoid full exhaustive matching. With known GPS positions of the images, shortlists of nearby images can be created and matching can be performed on the shortlist images only, avoiding to try to matchimages that will not match anyway. In the two-view geometry estimation step, the essential matrix between two images is computed to verify the matching. This requires the use of the 5 pt essential matrix estimation algorithm within a RANSAC loop. For the case of MAVs when the gravity normal is measured by the IMU, the more efficient 3 pt essential matrix algorithm, which was developed exactly for this case, can be used instead of the 5 pt algorithm. The 3 pt algorithm takes two rotation measurements directly from the IMU and only estimates the three unknowns that remain. To do this, the algorithm only needs 3 pt correspondences, whereas the 5 pt method needs 5 pt correspondences. This is the reason for its much higher efficiency. When used within a RANSAC loop, the necessary RANSAC iterations depend exponentially on the number of point correspondences, thus 3 or 5 necessary point correspondences make a big difference in runtime.

In the triangulation and pose estimation step, nearby images are registered to each other and 3D points are triangulated. Image registration typically works by computing a pose hypothesis from 3D to 2D matches with the PnP algorithm and then to a nonlinear optimization of the re-projection error. Computing the pose hypothesis from 3D to 2D matches typically is a time-consuming algorithm and can be replaced when IMU measurements of the MAV are used. The pose hypothesis can be computed by integrating IMU measurements of the MAV into a pose hypothesis. It has been shown that such kind of pose hypothesis is sufficiently accurate for subsequent state-of-the-art nonlinear pose optimization.

In the final step, bundle adjustment is used to optimize all the camera poses and 3D points in a global manner. Here, the GPS and IMU measurements can be used as additional constraints, as global pose constraints (GPS), global attitude constraints (IMU), and relative orientation constraints (IMU).

Many of these concepts have already been implemented in the open-source SfM software MAVMAP (see Schönberger et al. (2014)), which has specifically been designed for 3D mapping using MAVs. The main benefits are a major speedup for important computer vision algorithms and increased robustness. In areas with low texture, which leads to a low number of extracted image features, additional information such as GPS or IMU will provide valuable constraints for solving the camera pose estimation problem. Image data from MAVs can be acquired in a highly irregular manner, which typically puts the SfM system to the test, and therefore, the SfM process will benefit a lot from using additional constraints. Figure 3.10 shows a 3D map of a rubble field used for the training of search and rescue personal. The 3D map has been computed from more than 3000 images taken by multiple low-flying MAVs. The camera poses have been computed using MAVMAP and the point cloud has been densified using SURE (see Rothermel et al. (2012)). Figure 3.10a depicts the trajectories of the MAVs in red. In this image, the irregularity of the acquisition path is clearly visible. Figure 3.10b shows a detailed view of a part of the 3D map from a viewpoint originally not observed from the MAVs.

3.5 Autonomous Navigation

Digital cameras most likely will be the most important sensors for autonomous navigation of MAVs. Digital cameras allow to map the environment of a MAV in 3D using sophisticated computer vision algorithms. Other options for 3D data generation, including laser range-finders, radar, ultrasound, infrared, still come with significant disadvantages. Full 3D laser range-finders, for instance, are currently simply too heavy to be put on small MAVs. Digital cameras, however, already today, weigh only mere grams and are incredibly tiny. Further miniaturization of both digital cameras and more importantly computer technology will allow to run ever more complex computer vision algorithms onboard of the MAV. Depth cameras in this context are also counted toward the digital camera class, as Kinect-style depth cameras resemble a camera-projector system. Online 3D mapping is a key ability to perform autonomous navigation. The MAV needs to sense obstacles and its surrounding environment in more detail to do path planning and obstacle avoidance for safe navigation. The challenge is real-time, high-fidelity depth sensing on a resource-limited platform with strict weight limitations. The current trend is to use specialized hardware embedded in the sensor to let computer vision algorithms such as stereo processing run in real time. The Kinect sensor, for example, is performing depth estimation within a system-on-a-chip (SoC) system. In other works, the semi-global matching algorithm for stereo processing is running on an FPGA chip to achieve real-time depth sensing for MAVs. The integration of ever more complex algorithms into sensor hardware is already in the works. For example, the Skybotix VI-sensor has its goal to put a complete visual-inertial pose estimation system into the sensor hardware itself, utilizing FPGA processing and dedicated hardware.

The sensor measurements have to be fused together into a common map representation of the environment that facilitates path planning, obstacle avoidance, and higher level AI operations such as exploration strategies. In this context, methods that have been developed for ground robots cannot directly be used, they have to be first extended from 2D representation to 3D representation as a MAV can navigate freely in 3D. 3D occupancy grids represent the state of the art of environment representation. They allow the incremental fusion of different depth scans into a common map of user-defined accuracy. 3D occupancy maps can efficiently be used for path planning and navigation. However, one quickly runs into scalability problems on memory-limited computers onboard of MAVs and typically tiling approaches have to be developed.

Figure 3.11 shows a 3D occupancy grid used for MAV navigation as described in Heng et al. (2014). Grid cells that are occupied by an object (walls, floor, furniture, etc.) are visualized as blue cubes. The occupancy grid was computed by fusing a large number of individual depth sensor measurements. Each grid cell further holds a probability that indicates the certainty about themeasurement.

3.6 Scene Interpretation

In the previous section, the focus was on how to use digital images for MAV navigation and mapping mainly by extracting 3D information from image data. However, the analysis of the image content itself by computer vision methods to perform scene interpretation is of importance for MAVs as well. Fully autonomous task-driven operation of a MAV needs knowledge about not only the geometry of its environment but also a semantic description and interpretation. The MAV needs to be able to detect and identify objects, people, and so on. It needs to be able to semantically localize itself, that is, recognize if it is in a kitchen, bathroom, or living room. It needs to identify people and understand gestures to interact with them. And it needs to be able to identify passageways, doors, windows, or exits. Computer vision is the most promising technology to achieve this necessary semantic interpretation.

Figure 3.12 shows an example for scene interpretation by a MAV (see Meier et al. (2012)). The system uses a face detector to detect and identify persons in its vicinity. This lets the system always keep a safe distance from people. The system also identifies objects from a pre-trained database. One of these objects is the exit sign attached above the door. The system can use information like this for path planning and navigation.

3.7 Concluding Remarks

Computer vision proved to be a key enabling technology for autonomous MAVs. Digital cameras are used for pose estimation and for navigation of MAVs using onboard sensors. The possibility to do pose estimation of a MAV with onboard sensors immensely widened the scenarios in which MAVs can be used. Without onboard pose estimation, autonomous operation would be restricted to outdoor areas where GPS reception is available or to instrumented areas (e.g., areas equipped with a tracking system). Applications like search and rescue in partially collapsed buildings, plant inspection tasks, or indoor metrology for documentation would otherwise just not be possible. In addition, the emergence of autonomous MAV can be considered as a new driving force for new developments in computer vision. Computer vision systems used for autonomous MAVs need to be highly reliable and robust. Many computer vision algorithms still lack in this respect. One promising route to more reliable computer vision algorithms is visual-inertial fusion, which is currently heavily investigated. Interest in this direction is largely due to the fact that a MAV is naturally equipped with an IMU and as such there is a big incentive to look into these approaches further. And of course, new insights into computer vision algorithms for MAVs can readily be transferred to other kinds of autonomous systems as well.