Mathematical Problems in Engineering

Volume 2012, Article ID 676385, 26 pages

http://dx.doi.org/10.1155/2012/676385

## Monocular SLAM for Visual Odometry: A Full Approach to the Delayed Inverse-Depth Feature Initialization Method

^{1}Department of Computer Science, CUCEI, Universidad de Guadalajara, 44430 Guadalajara, JAL, Mexico^{2}Department of Automatic Control, UPC, BarcelonaTech, 08034 Barcelona, Spain

Received 20 March 2012; Revised 6 July 2012; Accepted 8 July 2012

Academic Editor: Piermarco Cannarsa

Copyright © 2012 Rodrigo Munguía and Antoni Grau. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

This paper describes in a detailed manner a method to implement a simultaneous localization and mapping (SLAM) system based on monocular vision for applications of visual odometry, appearance-based sensing, and emulation of range-bearing measurements. SLAM techniques are required to operate mobile robots in *a priori* unknown environments using only on-board sensors to simultaneously build a map of their surroundings; this map will be needed for the robot to track its position. In this context, the 6-DOF (degree of freedom) monocular camera case (monocular SLAM) possibly represents the harder variant of SLAM. In monocular SLAM, a single camera, which is freely moving through its environment, represents the sole sensory input to the system. The method proposed in this paper is based on a technique called delayed inverse-depth feature initialization, which is intended to initialize new visual features on the system. In this work, detailed formulation, extended discussions, and experiments with real data are presented in order to validate and to show the performance of the proposal.

#### 1. Introduction

The online robot estimation position from measurements of self-mapped features is a class of problem, in the robotics community, known as simultaneous localization and mapping (SLAM) problem. This technique consists in increasingly building a consistent map of the environment and, at the same time, localizing the robot’s position while it explores its world. SLAM is perhaps the most fundamental problem to solve in robotics in order to build truly autonomous mobile robots.

Robot sensors have a large impact on the algorithm used in SLAM. Early SLAM approaches focused on the use of range sensors as sonar rings and lasers, see [1, 2]. Nevertheless there are some disadvantages with the use of range sensors in SLAM: correspondence or data association becomes difficult, they are expensive, and some of them are limited to 2D maps and computationally inefficient when the number of features is large (see [3, 4] for a complete survey).

The aforementioned issues have propitiated that recent works move towards the use of cameras as the primary sensing mode. Cameras have become more and more interesting for the robotics research community, because they yield a lot of valuable information for data association. A wide variety of algorithms can be obtained from the computer vision research community in order to extract high-level primitives from images. Those primitives are matched with the primitives stored in the map allowing thus the data association. This is still an open problem. A camera is a sensor that yields 3D information. Even for indoor robots whose pose can be represented in 2D, the ability to gather 3D information of the environment is essential. Cameras are also suitable for embedded systems: they are lightweight, cheap, and power saving. Using vision, a robot can locate itself using as landmarks common objects that encounters along its path.

In this context, the 6-DOF (degrees of freedom) monocular camera case (monocular SLAM) possibly represents the most difficult variant of SLAM; in monocular SLAM, a single camera can be freely moving by its environment representing the sole input sensor to the system. On the other hand, while range sensors (e.g., laser) provide range and angular information, a camera is a projective sensor which measures the bearing of image features. Therefore, depth information (range) cannot be obtained in a single frame. This drawback has triggered the emergence of especial techniques for feature initialization approaches in order to allow the use of bearing sensors (such as cameras) in SLAM systems.

As computational power grows, an inexpensive monocular camera can be used to perform simultaneously range and appearance-based sensing replacing typical sensors for range measurement (laser and sonar rings) and for dead reckoning (encoders). Thus, a camera connected to a computer becomes a position sensor which could be applied to different fields such as robotics (motion estimation basically in humanoids robots), wearable robotics (motion estimation for camera-equipped devices worn by humans), tele-presence (head motion estimation using an outward-looking camera), or television (camera motion estimation for live augmented reality) [5].

Monocular SLAM is closely related to the structure-from-motion (SFM) problem for reconstructing scene geometry. SFM techniques, which originally come from the computer vision research community, are sometimes formulated as off-line algorithms that require batch, simultaneous processing for all the images acquired in the sequence. Nevertheless, several recursive solutions to the SFM problem can be found in the literature. In this sense, one of the first works was presented by Matthies et al. in [6] though in this work it is assumed that the camera motion is known. By contrast, Gennery in [7] proposes a method to estimate motion from a known structure. A method for addressing the problem of the two above works, unknown structure as unknown motion, was first introduced by Azarbayejani et al. in [8]. Other SFM-based techniques are presented in works [9, 10]. Some hybrid techniques (SFM-Kalman Filtering) based on stereovision, as stated in [11], have also appeared.

Monocular SLAM has received much attention in the last years. In [12], Deans and Martial proposes a combination of a global optimization BA (bundle adjustment) for feature initialization and a Kalman filter for state estimation. Strelow proposes in [13] a similar method but mixing camera and inertial sensors measurements in an iterated extended Kalman filter (IEKF). In [14], Bailey proposes a variant of constrained initialization for bearing-only SLAM, where past estimates for the vehicle pose are retained in the SLAM state and thus feature initialization can be deferred until its estimates become well-conditioned. Moreover there are some works that use other estimation techniques (apart from EKF) like particle filters (PF) methods. In [15, 16], Kwok and Dissanayake propose a method based in particle filtering techniques. In this case the initial state of features is approximated using a sum of Gaussians, which defines a set of hypothesis for landmark position. In subsequent steps, bad hypotheses of depth are pruned while observations are done. A similar approach is presented in [17] for bearing-only tracking.

Davison et al., [5, 18], proposes a real-time method based on the well-established EKF framework as the main estimation technique. In those works a Bayesian partial initialization scheme for incorporating new landmarks is used. A separate particle filter, which is not correlated with the rest of the map, is used in order to estimate the feature depth prior to its inclusion in the map. It is important to note that prior to Davison, the feasibility of real-time monocular SLAM was first demonstrated by Jin et al. in [19].

In [20], Smith et al. presents a real-time method based on straight lines detection using the EKF estimator. Jensfelt et al. in [21] presents a method where the main idea is to let the SLAM estimation lag behind frames; those frames will determine which points are good landmarks to find an estimation of their 3D location. In [22], Solà et al. presents a method based on federate Kalman filtering technique. With an initial probability distribution function (PDF) for the features, a geometric sum of Gaussians is defined. The method is an approximation of the Gaussian sum filter (GSF) that permits undelayed initialization with an additive simple growth of the problem size. In [23] Lemaire et al. present a similar method to [22] but the features are initialized with a delayed method.

In [24], Eade and Drummond proposes a FastSLAM-based [25] approach. In that work the pose of the robot is represented by particles and a set of Kalman filters refines the estimation of the features. When the inverse depth collapses, the feature is converted into a fully initialized standard Euclidean representation. Montiel et al. in [26] present a new approach where the transition from partially to fully initialized features does not need to be explicitly tackled, making it suitable for direct use in EKF framework in sparse mapping. In this approach, the features are initialized in the first frame they are observed with an initial, fixed inverse depth and uncertainty heuristically determined to cover the range from nearby to infinite; therefore distant points can be coded.

In delayed methods, a feature observed in the instant is added to the map in a subsequent time . Usually the delay is used, in this kind of methods, for collecting information that permits to initialize the feature in a robust manner. On the other hand, the undelayed methods take advantage of the observation of the feature from the instant for updating the system. Conversely the system update step should be computed carefully.

In authors’ previous works, [27, 28], a monocular SLAM approach is proposed. In this paper a method, the so-called delayed inverse-depth feature initialization, is used to initializing new features in the system. This method, which is based on the inverse depth parameterization, defines a single hypothesis for the initial depth of features by the use of a stochastic technique of triangulation.

This paper presents an extended and detailed version of the monocular SLAM scheme proposed in [27, 28]. Though the fundamental idea of the original method remains the same, some important and remarkable improvements have been made and included in this paper. Moreover, the implementation of the proposed method is presented in a very detailed manner. New experiments with real data and discussion are also included in order to show the performance of the proposed method and its importance as an open problem in autonomous robotics engineering.

#### 2. Method Description

In this section, the proposed scheme for monocular SLAM is described. In Figure 1 a block diagram indicating the flow information and main sub-processes of the system is shown. All its components are explained in detail in subsequent sub-sections. It is important to remark that once the system is initialized, the detection and measurement of image features represents the sole sensory input of the system.

##### 2.1. System Parameterization

The complete system state consists of: where represents the state of a free robot camera moving in any direction in , and represents a feature point in the 3D scene. At the same time, is composed of: where represents the camera optical center position in Cartesian coordinates, and: represents the orientation of the camera respect to the navigation frame by a unit quaternion. Unit quaternions provide a convenient mathematical notation for representing orientations and rotations of objects in three dimensions. Compared with Euler angles they are simpler to compose avoiding the problem of gimbal lock. Compared with rotation matrices they are numerically more stable and in many cases they are more efficient. A good review for attitude representations is given in [29]. The following expressions: denote linear and angular velocities, respectively. The superscripts and denote magnitudes expressed in the world reference, and in the camera reference respectively.

A feature is composed of the following 6-dimension state vector: which models the 3D point located at: where , , corresponds to the optical center coordinates of the camera when the feature was observed for the very first time; and , represent the azimuth and the elevation respectively (respect to the world reference ) for the directional unitary vector :

The point depth is coded by its inverse value, = , as quoted in [26]. Figure 2 illustrates the camera and features parameterization.

##### 2.2. System Initialization

In a robotics context, obtaining the metric scale of the world can be very useful. However, in monocular SLAM the scale of the observed world cannot be obtained using only vision, and therefore another sensor or the observation of a known dimension reference have to be used in order to retrieve the scale of the world.

In this work, the system metric initialization process is analogous to a classical problem in computer vision: the perspective of -point (PnP) problem [30]. The PnP problem is to find the position and orientation of a camera with respect to a scene object from correspondent points. In [30] it is demonstrated that, when the control points are coplanar, the perspective on 4-point (P4P) problem has a only solution.

Therefore, in the present work it is assumed that 4 coplanar points are known (for instance, the dimensions of a black paper sheet over a white background). It is also assumed that the intrinsic parameters of the camera are already known: (i) (focal length); (ii) , (displacement to the image center), and (iii) (radial lens distortion).

The problem consists in estimating two extrinsic camera parameters: (world to camera rotation matrix for camera orientation) and (translation vector for the position of the camera center), given 4 coplanar points with spatial coordinates (, , 0), with in [1..4], and their corresponding 4 undistorted image coordinates (), as shown in Figure 3.

In order to estimate and , the method is based in the approach proposed in [31]. The following system of linear equations is formed with the vector as an unknown parameter: where The linear system in (2.9) is solved for . Then is obtained as follows: The extrinsic parameters and are obtained using the following equations:

In (2.12) the third column of matrix is formed by the combinations of the values of first and second column of the same matrix. A good review of algorithms for coplanar camera calibration can be found in [29].

At the beginning of the iterative process, the system state is formed by the camera-state , and the four initial features used for estimating the extrinsic camera parameters: where has the same value that the extrinsic parameter calculated by (2.13). is estimated using the rotation matrix to quaternion transformation , described by (A.2) in the Appendix, from the transpose of the rotation matrix obtained by (2.12).

Each initial feature with in [1..4] is composed by: where is a two-argument function that computes the arctangent of for given values of and , within the range [, ]. If certain confidence is assumed in the estimation process for the initial system state , then the initial covariance matrix of the system can be filled with zeros or , being this value an arbitrary very small positive value:

##### 2.3. Camera Motion Model and Filter Prediction Equations

An unconstrained prediction model for a constant-acceleration camera motion can be defined by the following equation [5, 32]: being the quaternion computed from the rotation vector . This transformation from rotation vector to quaternion is defined by (A.1) in the Appendix.

No prior assumption can be made about the camera movement. Therefore, the model defined in (2.19) supposes, at every step , that the changes in linear and angular velocity and are produced by an input of unknown linear and angular velocity and , with linear and angular acceleration with zero-mean and known Gaussian process covariance, and , respectively:

The unknown linear and angular velocity and are incorporated into the system by the process noise covariance matrix :

If a static scene is assumed where the landmarks remain in a fixed place, the prediction step for the Extended Kalman Filter (EKF) is defined as follows: where and the Jacobians and are defined as follows: are the derivatives of the equations of the prediction model with respect to the camera state . are the derivatives of the equations of the prediction model with respect to the parameters of the covariance matrix . In this case corresponds to the dimension of the features in the system state .

##### 2.4. Measurement Prediction Model

The different locations of the camera, along with the location of the features already mapped, are used by the measurement prediction model to forecast the distorted pixel coordinates and for each feature :
is derived using the following procedure: the model observation of any point from a camera location defines a ray expressed in the camera frame as follows, see Figure 2:
is observed by the camera through its projection in the image. is the transformation matrix from the global reference frame to the camera reference frame. * is *computed by where is a rotation matrix depending on the camera orientation quaternion using (A.3) which computes a rotation matrix from a quaternion. The directional unitary vector is computed with (2.8).

The vector is projected to the normalized camera retina by the following: The camera calibration model is applied to produce the undistorted pixel coordinates and : where is the camera center in pixels, and is the focal length. Finally, a radial distortion model is applied to obtain the distortion pixel coordinates . In [18] the following distortion model is proposed being the distortion coefficient:

##### 2.5. Features Matching and Filter Update Equations

If the feature is predicted to be appeared in the image ( image height & image width), then an active search technique [33] can be used to constrain the features matching from frame to frame inside elliptical regions around the predicted = []. The elliptical regions are defined by the innovation covariance matrix :

Measurement noise is incorporated into the system by the noise covariance matrix :

Variances and are defined in pixel units. is the prior state covariance estimated by (2.23). is the Jacobian derived from the measurement model : are the derivatives of the equations of the measurement prediction model with respect to the camera state . are the derivatives of the measurement prediction model with respect to the parameters of the feature . Note that has only a nonzero value at the location (indexes) of the observed feature .

The size of the axis for the elliptical search region defined by the matrix is determined by (see Figure 4): where is the number of standard deviations for the desired region of search.

When a feature is added to the map, a unique image patch of by pixel is stored and linked with the feature. For matching a feature in the current image frame, a patch cross-correlation technique, [33], is applied over all the image locations () within the search region defined in (2.33). If the score of a pixel location (), determined by the best cross correlation between the feature patch and the patches defined by the region of search, is higher than a threshold then this pixel location () is considered as the current feature measurement .

If the matching process is successful, then the filter is updated as follows: where the innovation is as follows: and is the Kalman gain:

In SLAM, it is well known the injurious effect of incorrect or incompatible matches. In monocular SLAM systems, when delayed feature initialization techniques are used (as in this work), implicitly some weak image features are pruned prior to their addition to the stochastic map, for example, image features produced by fast lighting changes, shines in highly reflective surfaces, or even caused by some dynamic elements in the scene. Nevertheless, the risk of incorrect or incompatible matches could remain due to several factors as: (i) incompatibility due repeated design; (ii) fake composite landmark; (iii) incompatibilities produced by reflections on curved surfaces and materials; (iv) detection of landmarks running along edges. The above drawbacks can be mitigated by the used of batch validation techniques [34, 35] (or variants of them), with the penalty of the increment of computational time.

On the other hand, for the standard EKF-SLAM algorithm, there exists significant empirical evidence which demonstrates that the computed state estimates tend to be inconsistent. Thus, if the covariance matrix is underestimated, then the search regions determined by the active search technique could even collapse, leading the tracking to fail. A feasible solution to this problem could be implemented by fixing a minimum possible size for the axes , in (2.33).

##### 2.6. Delayed Inverse-Depth Feature Initialization

As it is stated before, depth information cannot be obtained in a single measurement when bearing sensors (e.g., a single camera) are used. To infer the depth of a feature, the sensor must observe this feature repeatedly as it freely moves through its environment, estimating the angle from the feature to the sensor center. The difference between those angle measurements is the parallax feature. Actually, parallax is the key that allows estimating features depth. In case of indoor sequences, a displacement of centimeters is enough to produce parallax; on the other hand, the more distant the feature, the more the sensor has to travel to produce parallax. For incorporating new features to the map, special techniques for features initialization are needed in order to enable the use of bearing sensors in SLAM systems.

In this work, authors propose a novel method called delayed inverse-depth feature initialization in order to incorporate new features to the stochastic map from the bearing measurements of the camera. The proposed method uses the inverse-depth parameterization and implements an stochastic technique of triangulation, by means of delay, in order to define hypothesis of initial depth for the features. The use of the inverse-depth parameterization, in the context of motion estimation and scene reconstruction using vision, was introduced earlier by Favaro in [36], and extended later in [24, 26].

The proposed method states that a minimum number of features is considered to be predicted appearing in the image, otherwise new features should be added to the map. In this latter case, new points are detected in the image with a saliency operator. In this particular case, Harris corner detector is applied, but other detectors should also be used.

These points in the image, which are not added yet to the map, are called candidate points, . Only image areas free of both, candidate points and mapped features , are considered for detecting new points with the saliency operator, see Figure 5.

When a candidate point is detected for the first time, some data are stored: where is taken from the current camera optical center position : represents the associated variances of , taken from the state covariance matrix : is taken from the quaternion representing the current camera orientation: represents the associated variances of , taken from the state covariance matrix : and are the initial pixel coordinates where the candidate point was detected for the first time. In subsequent frames, each candidate point is intended to be tracked (this approach is independent of the used tracking method) in order to maintain the pixel coordinates and updated for such a candidate point. In practice, not all the candidate points can be tracked, nevertheless the method takes advantage of this fact in order to prune weak image features. For each candidate point , the tracking process is realized until the point is pruned or initialized in the system as a new feature . In practice for each frame, the candidate points could be detected, pruned or considered to be added to the map.

A candidate point is added as a new feature when a minimum threshold of parallax is reached. Parallax is estimated using (as it can be seen in Figure 6):(i)displacement base-line ;(ii) and associated data;(iii)the current camera state and current measurement .

For each candidate point , when a new measurement is available then parallax angle is estimated by the following: The angle is determined by the directional unitary vector and the vector defining the base-line in the direction of the camera trajectory by the following: where the directional projection ray vector , expressed in the absolute frame , is computed from the camera position and the coordinates of the observed point when it was observed for the first time, using the data stored in . is the transformation matrix from the camera reference frame to the global reference frame, depending on the stored quaternion ; (A.3) is used for computing a rotation matrix from a quaternion.

is the directional vector, in the camera frame , pointing from the position when the candidate point was observed for the first time to the feature location, and it can be computed by the following: The undistorted pixel coordinates and are obtained from applying the inverse of the distortion model in (2.29): is the vector representing the camera base-line between the camera optical center position , where the point was observed for the first time, and the current camera optical center , taken from the current camera state : The angle is determined in a similar way as but using instead the directional projection ray vector and the vector :

The directional projection ray vector expressed in the absolute frame , is computed in a similar way as but using the current camera position and therefore, the current undistorted points coordinates : is the transformation matrix from the camera reference frame to the global reference frame, depending on the current quaternion . Equation (A.3) is used for computing a rotation matrix from a quaternion.

is the directional vector, in the camera frame , pointing from the current camera position to the feature location, and it can be computed as follows: The undistorted coordinates pixel and are obtained from the current pixel coordinates , applying the inverse of the distortion model in (2.29): is similar to but pointing to the opposite direction: The base-line is the module of or : If then is initialized as a new feature map . The feature is made up of: The first three elements of are obtained directly from the current camera optical center position taken from : where is computed using (2.49).

The inverse depth is derived from the following: The new system state is made up simply adding the new feature to the final part of the vector state . If then is composed by the following: The new state covariance matrix is derived from the error diagonal measurement covariance matrix and the current state covariance matrix : where is composed by the measurement error variances and (defined in pixel units), and the variances stored with the candidate point .

The new state covariance matrix after the initialization process is given by the following: The Jacobian is composed of the following: where is a -dimensional identity matrix where is the dimension of . are the derivatives of the initializations equations with respect to the camera state . Note that the derivatives with respect to the previously mapped features are filled with zeros. are the derivatives of the initializations equations with respect to the parameters of the covariance matrix .

For certain environments, those features located far away from the camera will not produce parallax (for small trajectories with respect to the feature depth). To include features in the map with big uncertainty in depth is not very helpful for estimating the camera location , although this kind of features could provide useful information for estimating camera orientation .

A special case is when a near feature does not produce parallax. This case happens when the camera moves in the direction towards the feature position (e.g., ). In this work it is assumed that there exist the necessary conditions for producing parallax at every time at least for one feature observed by the camera. Furthermore, features located in front of the direction of the camera movement will be discarded (if ).

For the proposed method, regarding whether distant features should be included in the map, a minimum base-line can be used as an additional threshold for considering a candidate point to be initialized as a new feature . In this case, a candidate point is considered to be distant from the camera when but . Camera moved from its previous position but no enough parallax has been produced, due to the distant condition of the landmark.

The value of can be heuristically determined depending on the particularities of the application. In this case, for initializing a candidate point as a new feature , where but , the simple method for initializing new features proposed in [26] can be used. Nevertheless, in this work the following equations are proposed in order to determine initial values , depending on and :

##### 2.7. Map Management

When the number of features in the system state increases then computational cost grows rapidly and consequently it becomes difficult to maintain the frame rate operation. To alleviate this drawback, old features can be removed from the state for maintaining a stable number of features and, therefore, to stabilize the computational cost per frame. Obviously, if old features are removed, then previous mapped areas cannot be recognized in the future. However, in the context of visual odometry, this fact is not considered as a problem. A modified monocular SLAM method that maintains the computational operation stable can be viewed as a complex real-time virtual sensor, which provides appearance-based sensing and emulates typical sensors as laser for range measurements and encoders for dead reckoning (visual odometry).

In authors’ related work [37], the *virtual sensor* idea is proposed, developed, and extended so that, using a distributed scheme, it is possible to recognize previous mapped areas for loop closing tasks. In this scheme, the outputs provided by the virtual sensor have been used by an additional SLAM process (decoupled from the camera’s frame rate) in order to build and to maintain the global map and final camera pose.

This present work regards the approach of monocular SLAM as a virtual sensor. Therefore, if the number of features exceeds a specified threshold, in order to maintain a stable amount of features in the system the algorithm removes older features that were left behind by camera movement as well as those features that have been predicted to be appearing in the image, but have not been matched in several previous frames.

Removing features from the system state is much easier than adding them. To delete a feature from the state vectorand covariance matrix, the rows and columns which contain it have to be removed. As an example, the remaining matrices after the removal of feature will be the following:

#### 3. Experimental Results

The proposed method has been implemented using MATLAB in order to test its performance. First, several video sequences were acquired using two different low cost cameras. Later, the algorithm was executed off-line using those video sequences as input signals. In experiments, the following parameter values have been used: variances for linear and angular velocity, respectively, m/s^{2}, m/s^{2}, noise variances pixel, minimum base-line cm and minimum parallax angle .

In the first series of experiments, a Microsoft LifeCam Studio Webcam was used. This low cost camera has an USB interface and a wide angle lens. It is capable of acquiring HD color video, but in the present experiments gray level video sequences, with a resolution of 424 × 240 pixels at 30 frames per second, were used.

Figure 7 shows the experimental results for a video sequence of 790 frames acquired in a desktop environment with the LifeCam Studio webcam. In this case, the camera was moved from the left to the right following a trajectory similar to the curved front edge of the desktop (see Figure 9, left side). A black paper sheet was used as the initial metric reference (Frame 1, Figure 7). It is important to remark that the initial metric reference determines the origin of the world reference. The initial camera position and orientation respect to the coordinates of the world reference frame are determined according Section 2.2 explanations. Some candidate points were detected from the first frame; however, the first feature was not added to the map until Frame 111. In this frame, two new candidate points were detected (in red), and two candidate points were tracked (in blue). For simplicity, feature depth uncertainty is indicated by red 3D lines in plots illustrating the outputs of the proposed method (central and right columns in Figure 7). The features are located over these red lines. Of course, full geometric uncertainties should be represented by 3D ellipsoids. In frame 203 the first features, corresponding to the initial metric reference, have been left behind by the camera movement. Other features, corresponding to a painting placed approximately in the same plane that the initial metric reference, have been initialized. In frame 533 several features, corresponding to two different PC screens, have been added to the map. Weak image features, due for example to brightness over reflecting surfaces or changing video in the PC screens, are implicitly pruned by the image tracking process of candidates points. At frame 688, several features have been left behind by the camera movement and removed from the system (in yellow), while other features close to the range camera view, are maintained in the map (in red).

Figure 8 shows a top view of the experimental environment (left plot), and the final camera trajectory and map estimates, in an view at frame 790 (right plot). Several objects have been labeled in both plots for illustrating the reconstruction of the scene structure. In this experiment both, camera trajectory and scene structure, have been recovered in a satisfactory manner.

In the second series of experiments, a Fire-i Unibrain monochrome webcam was used. This low-cost camera has an IEEE1394 interface and interchangeable wide angle lens. Video sequences with a resolution of 320 × 240 pixels acquired at 30 frames per second were used in experiments. Figure 9 shows the experimental results for a video sequence containing about 1750 frames. This sequence has been recorded walking over a predefined cyclical trajectory inside a laboratory environment. At the end of the sequence, 390 features were initialized in the map, but only about 20 or 30 features in average were maintained in the system state allowing that the computational time per frame was stable along the sequence.

In this experiment, the only reference for recovering the metric scale is a computer screen (frame 3), that was left behind around frame 300 by the camera movement. In the frame 565, some features have been clearly removed from the map. Here a feature is removed if it cannot be detected and tracked in the next 20 frames. In the frame 777 the camera has completed its first turn (or cycle) to the defined trajectory. In the frame 1635 the camera has returned next to its initial position (note that the PC screen used as initial metric reference appears again). At this point the discrepancy between the estimated trajectory and the real trajectory is very perceptible (the real path followed is like a long rectangle with rounded edges). The biggest degeneration in the estimated trajectory happens when the camera is turning the curves, mainly in the second and third curve, because there are not far features to obtain enough information for the orientation. Without underestimating the importance of the effects produced by the drift in the trajectory estimations, it can be appreciated in Figure 10 that the 3D structure of the environment has been recovered reasonably well.

#### 4. Conclusions

In this paper a method for addressing the problem of 3D visual SLAM in a context of visual odometry using a single free-moving camera has been presented.

Depth information cannot be obtained in a single measurement when a bearing sensor (i.e., monocular camera) is used. Therefore, in order to incorporate new features to the map, special techniques for feature system initialization are needed in order to enable the use of bearing sensors in SLAM systems. In this work a method called delayed inverse-depth feature initialization is proposed to incorporate new features to the stochastic map from the bearing measurements of the camera. The proposed method is based in the inverse depth parameterization and implements, by means of a delay, a stochastic technique of triangulation in order to define a hypothesis of initial depth for the features.

In SLAM methods based on the extended Kalman filter is difficult to maintain the frame rate operation when the number of features in the system state increases. Old features have to be removed from the state in order to maintain a stable computational cost per frame. Obviously, if features are removed then previous mapped areas cannot be recognized in the future. However, a modified monocular SLAM method to maintain stable computational operation can be viewed as a complex real-time virtual sensor, which provides appearance-based sensing and emulates typical sensors (i.e., a laser) for range measurements and encoders for dead reckoning (visual odometry).

The experimental results, with real data, show that although a small and single metric reference is used for recovering the world scale, the metric degeneration quantification at the end of the trajectory is low. In the experiments carried out, the camera trajectory and scene structure have been recovered in a satisfactory manner. The results in the trajectory are not error-free though they are more likely to appear in curves when distant features do not exist.

Even though this work is concerned with the approach of monocular SLAM as a virtual sensor (e.g., in the context of visual odometry), the effectiveness of the proposed method has been also verified in a more general SLAM context. A real time implementation of the proposed method [37], fully adapted as a virtual sensor, provides a rich sensory input to a decoupled (Global) SLAM process. This Global SLAM is used for loop closing tasks and therefore for building and maintaining the global map and final camera pose.

#### Appendix

In this appendix some transformations, repeatedly used along the paper, are included.

A quaternion can be computed from a rotation vector by the following: A quaternion can be computed form a rotation matrix by the following: A rotation matrix can be computed from a quaternion by the following:

#### Acknowledgments

This work has been funded by the Spanish Ministry of Science Project DPI2010-17112.

#### References

- F. Auat, C. De la Cruz, R. Carelli, and T. Bastos, “Navegación autónoma asistida basada en SLAM para una silla de ruedas robotizada en entornos restringidos,”
*Revista Iberoamericana de Automática e Informática Industrial*, vol. 8, pp. 81–92, 2011. View at Google Scholar - R. Vázquez-Martín, P. Núñez, A. Bandera, and F. Sandoval, “Curvature-based environment description for robot navigation using laser range sensors,”
*Sensors*, vol. 9, no. 8, pp. 5894–5918, 2009. View at Publisher · View at Google Scholar · View at Scopus - H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: part I,”
*IEEE Robotics and Automation Magazine*, vol. 13, no. 2, pp. 99–110, 2006. View at Publisher · View at Google Scholar · View at Scopus - T. Bailey and H. Durrant-Whyte, “Simultaneous localization and mapping: part II,”
*IEEE Robotics and Automation Magazine*, vol. 13, no. 3, pp. 108–117, 2006. View at Publisher · View at Google Scholar · View at Scopus - A. J. Davison, “Real-time simultaneous localisation and mapping with a single camera,” in
*Proceedings of the 19th IEEE International Conference on Computer Vision (ICCV '03)*, vol. 2, pp. 1403–1410, October 2003. View at Scopus - L. Matthies, T. Kanade, and R. Szeliski, “Kalman filter-based algorithms for estimating depth from image sequences,”
*International Journal of Computer Vision*, vol. 3, no. 3, pp. 209–238, 1989. View at Publisher · View at Google Scholar · View at Scopus - D. B. Gennery, “Visual tracking of known three-dimensional objects,”
*International Journal of Computer Vision*, vol. 7, no. 3, pp. 243–270, 1992. View at Publisher · View at Google Scholar · View at Scopus - A. Azarbayejani, B. Horowitz, and A. Pentland, “Recursive estimation of structure and motion using relative orientation constraints,” in
*Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition*, pp. 294–299, June 1993. View at Scopus - H. Jin, P. Favaro, and S. Soatto, “A semi-direct approach to structure from motion,”
*Visual Computer*, vol. 19, no. 6, pp. 377–394, 2003. View at Publisher · View at Google Scholar · View at Scopus - A. W. Fitzgibbon and A. Zisserman, “Automatic camera recovery for closed or open image sequences,” in
*Proceedings of the European Conference on Computer Vision (ECCV '98)*, vol. 1, pp. 311–326, 1998. - Y. K. Yu, K. H. Wong, S. H. Or, and M. M. Y. Chang, “Robust 3-D motion tracking from stereo images: a model-less method,”
*IEEE Transactions on Instrumentation and Measurement*, vol. 57, no. 3, pp. 622–630, 2008. View at Publisher · View at Google Scholar · View at Scopus - H. Deans and M. Martial, “Experimental comparison of techniques for localization and mapping using a bearing-only sensor,” in
*Proceedings of the 7th International Symposium on Experimental Robotic (ISER '00)*, December 2000. - S. Strelow and D. Sanjiv, “Online motion estimation from image and inertial measurements,” in
*Proceedings of the Workshop on Integration of Vision and Inertial Sensors (INERVIS '03)*, June 2003. - T. Bailey, “Constrained initialisation for bearing-only SLAM,” in
*Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '03)*, pp. 1966–1971, September 2003. View at Scopus - N. M. Kwok and G. Dissanayake, “An efficient multiple hypothesis filter for bearing-only SLAM,” in
*Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '04)*, vol. 1, pp. 736–741, October 2004. View at Scopus - N. M. Kwok, G. Dissanayake, and Q. P. Ha, “Bearing-only SLAM using a SPRT based gaussian sum filter,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, pp. 1109–1114, April 2005. View at Publisher · View at Google Scholar · View at Scopus - N. Peach, “Bearings-only tracking using a set of range-parameterized extended Kalman filters,”
*IEE Proceedings*, vol. 142, no. 1, pp. 73–80, 1995. View at Publisher · View at Google Scholar · View at Scopus - A. Davison, Y. Gonzalez, and N. Kita, “Real-time 3D SLAM with wide-angle vision,” in
*Proceedings of the 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles (IAV '04)*, July 2004. - H. Jin, P. Favaro, and S. Soatto, “Real-time 3-D motion and structure of point features: a front-end system for vision-based control and interaction,” in
*Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR '00)*, pp. 778–779, June 2000. View at Scopus - P. Smith, I. Reid, and A. Davison, “Real-time monocular SLAM with straight lines,” in
*Proceedings of the British Machine Vision Conference*, BMVA Press, September 2006. - P. Jensfelt, J. Folkesson, D. Kragic, and H. Christensen, “Exploiting distinguishable image features in robotics mapping and localization,” in
*Proceedings of the European Robotics Symposium (EUROS '06)*, Palermo, Italy, 2006. - J. Solà, A. Monin, M. Devy, and T. Lemaire, “Undelayed initialization in bearing only SLAM,” in
*Proceedings of the IEEE IRS/RSJ International Conference on Intelligent Robots and Systems (IROS '05)*, pp. 2751–2756, August 2005. View at Publisher · View at Google Scholar · View at Scopus - T. Lemaire, S. Lacroix, and J. Solà, “A practical 3D bearing-only SLAM algorithm,” in
*Proceedings of the IEEE IRS/RSJ International Conference on Intelligent Robots and Systems (IROS '05)*, pp. 2757–2762, August 2005. View at Publisher · View at Google Scholar · View at Scopus - E. Eade and T. Drummond, “Scalable monocular SLAM,” in
*Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR '06)*, pp. 469–476, June 2006. View at Publisher · View at Google Scholar · View at Scopus - M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM: a factored solution to the simultaneous localization and mapping problem,” in
*Proceedings of the 18th National Conference on Artificial Intelligence (AAAI '02)*, pp. 593–598, Menlo Park, Calif, USA, August 2002. View at Scopus - J. M. M. Montiel, J. Civera, and A. Davison, “Unified inverse depth parametrization for monocular SLAM,” in
*Proceedings of the Robotics: Science and Systems Conference*, August 2006. - R. Munguia and A. Grau, “Monocular SLAM for visual odometry,” in
*Proceedings of the IEEE International Symposium on Intelligent Signal Processing (WISP '07)*, Alcala de Henares, Spain, October 2007. View at Publisher · View at Google Scholar · View at Scopus - R. Munguia and A. Grau, “Delayed inverse depth monocular SLAM,” in
*Proceedings of the 17th World Congress, International Federation of Automatic Control (IFAC '08)*, Seoul, Korea, July 2008. View at Publisher · View at Google Scholar · View at Scopus - M. D. Shuster, “Survey of attitude representations,”
*The Journal of the Astronautical Sciences*, vol. 41, no. 4, pp. 439–517, 1993. View at Google Scholar · View at Scopus - C. Chatterjee and V. P. Roychowdhury, “Algorithms for coplanar camera calibration,”
*Machine Vision and Applications*, vol. 12, no. 2, pp. 84–97, 2000. View at Publisher · View at Google Scholar · View at Scopus - S. Ganapathy, “Decomposition of Transformation Matrices for Robot Vision,” in
*Proceedings of the International Conference on Robotics and Automation*, pp. 130–139, 1984. - A. Chiuso, P. Favaro, H. Jin, and S. Soatto, “MFm: 3-D motion from 2-D motion causally integrated over time,” in
*Proceedings of the European Conference on Computer Vision (ECCV '00)*, 2000. - A. J. Davison and D. W. Murray, “Mobile robot localisation using active vision,” in
*Proceedings of the 5th European Conference on Computer Vision (ECCV '98)*, vol. 2, pp. 809–825, Freiburg, Germany, 1998. - J. Neira and J. D. Tardós, “Data association in stochastic mapping using the joint compatibility test,”
*IEEE Transactions on Robotics and Automation*, vol. 17, no. 6, pp. 890–897, 2001. View at Publisher · View at Google Scholar · View at Scopus - T. Baile,
*Mobile robot localisation and mapping in extensive outdoor environments [Ph.D. thesis]*, University of Sydney, Australian Centre for Field Robotics, 2002. - P. Favaro,
*Stima del moto e della struttura della scena tramite visione dinamica [M.S. thesis]*, University of Padua, 1998. - R. Munguía and A. Grau, “Closing loops with a virtual sensor based on monocular SLAM,”
*IEEE Transactions on Instrumentation and Measurement*, vol. 58, no. 8, pp. 2377–2384, 2009. View at Publisher · View at Google Scholar · View at Scopus