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

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.

Figure 1 

Block diagram showing the architecture of the system.

2.1. System Parameterization

The complete system state 𝑥̂ consists of: 𝑥̂=𝑥̂_𝑣,𝑦₁̂,𝑦₂̂,…,𝑦_𝑛̂^𝑇,(2.1) where 𝑥̂_𝑣 represents the state of a free robot camera moving in any direction in ℝ³×SO(3), and 𝑦_𝑖̂ represents a feature point 𝑖 in the 3D scene. At the same time, 𝑥̂_𝑣 is composed of: 𝑥̂_𝑣=𝑟^𝑊𝐶𝑞^𝑊𝐶𝑣^𝑊𝜔^𝑊^𝑇,(2.2) where 𝑟^𝑊𝐶=𝑥_𝑣𝑦_𝑣𝑧_𝑣^𝑇,(2.3) represents the camera optical center position in Cartesian coordinates, and: 𝑞^𝑊𝐶=𝑞₁𝑞₂𝑞₃𝑞₄^𝑇(2.4) 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: 𝜔𝑣^𝑊_𝑊=𝑣_𝑥𝑣_𝑦𝑣_𝑧^𝑇,=𝜔_𝑥𝜔_𝑦𝜔_𝑧^𝑇,(2.5) 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: 𝑦_𝑖̂=𝑥_𝑖𝑦_𝑖𝑧_𝑖𝜃_𝑖𝜙_𝑖𝜌_𝑖^𝑇,(2.6) which models the 3D point located at: ⎡⎢⎢⎢⎢⎣𝑥_𝑖𝑦_𝑖𝑧_𝑖⎤⎥⎥⎥⎥⎦+1𝜌_𝑖𝑚𝜃_𝑖,𝜙_𝑖,(2.7) 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 𝑚(𝜃_𝑖,𝜙_𝑖): 𝑚𝜃_𝑖,𝜙_𝑖=cos𝜙sin𝜃−sin𝜙cos𝜙cos𝜃^𝑇(2.8)

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

Figure 2 

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.

Figure 3 

Perspective of 4 coplanar points (P4P) problem.

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: ⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣𝑥₁0𝑓𝑦₁0𝑓𝑥₁00−𝑖₁𝑥₁−𝑖₁𝑦₁𝑓0𝑥𝑓𝑦₁𝑓−𝑗₁𝑥₁−𝑗₁𝑦₁0𝑓₂0𝑓𝑦₂0𝑓𝑥₂00−𝑖₂𝑥₂−𝑖₂𝑦₂𝑓0𝑥𝑓𝑦₂𝑓−𝑗₂𝑥₂−𝑗₂𝑦₂0𝑓₃0𝑓𝑦₃0𝑓𝑥₃00−𝑖₃𝑥₃−𝑖₃𝑦₃𝑓0𝑥𝑓𝑦₃𝑓−𝑗₃𝑥₃−𝑗₃𝑦₃0𝑓₄0𝑓𝑦₄0𝑓𝑥₄00−𝑖₄𝑥₄−𝑖₄𝑦₄𝑓0𝑓𝑦₄𝑓−𝑗₄𝑥₄−𝑗₄𝑦₄0𝑓⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦𝑏=⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣𝑖¹𝑗₁𝑖₂𝑗₂𝑖₃𝑗₃𝑖₄𝑗₄⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦,(2.9) where 𝑏=𝑟¹¹𝑡₃𝑟₁₂𝑡₃𝑟₂₁𝑡₃𝑟₂₂𝑡₃𝑟₃₁𝑡₃𝑟₃₂𝑡₃𝑡₁𝑡₃𝑡₂𝑡₃^𝑇.(2.10) The linear system in (2.9) is solved for 𝑏=[𝑏₁𝑏₂𝑏₃𝑏₄𝑏₅𝑏₆𝑏₇𝑏₈]^𝑇. Then 𝑡₃ is obtained as follows: 𝑡₃=𝑓²𝑏²¹+𝑏²³+𝑓²𝑏²⁵.(2.11) The extrinsic parameters 𝑅^𝐶𝑊 and 𝑡 are obtained using the following equations: 𝑅^𝐶𝑊=⎡⎢⎢⎢⎢⎣𝑡³𝑡₃𝑏₁𝑡₃𝑏₂𝑅₂₁𝑅₃₂−𝑅₃₁𝑅₂₂𝑡₃𝑏₃𝑡₃𝑏₄𝑅₃₁𝑅₁₂−𝑅₁₁𝑅₃₂𝑡𝑏=₅𝑡₃𝑡𝑏₆₃𝑅₁₁𝑅₂₂−𝑅₂₁𝑅₁₂⎤⎥⎥⎥⎥⎦,(2.12)𝑏₇𝑡₃𝑏₈𝑡₃^𝑇.(2.13)

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 𝑥̂_ini is formed by the camera-state 𝑥̂_𝑣, and the four initial features used for estimating the extrinsic camera parameters: 𝑥̂_ini=𝑟^𝑊𝐶_ini𝑞^𝑊𝐶_ini𝑣^𝑊_ini𝜔^𝑊_ini𝑦₁̂𝑦₂̂𝑦₃̂𝑦₄̂^𝑇,(2.14) where 𝑟^𝑊𝐶_ini=𝑡,𝑞^𝑊𝐶_ini=𝜔𝑞_𝑊ini𝑅^𝐶𝑊^𝑇,𝑣^𝑊ini=0_3×1,=0_3×1,(2.15)𝑟^𝑊𝑅_ini has the same value that the extrinsic parameter 𝑡 calculated by (2.13). 𝑞^𝑊𝐶_ini 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: 𝑦_î=𝑟^𝑊𝐶_iniatan2𝑔₁,𝑔₃atan2−𝑔₂,𝑔²¹+𝑔²³1‖‖𝑟^𝑊𝐶_ini‖‖^𝑇,(2.16) where 𝑔₁𝑔₂𝑔₃=𝑥_𝑖𝑦_𝑖0−𝑟^𝑊𝐶_ini,(2.17)atan2 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 𝑥̂_ini, then the initial covariance matrix of the system 𝑃_ini can be filled with zeros or 𝜀, being this value an arbitrary very small positive value: 𝑃_ini=𝜀_37×37.(2.18)

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]: 𝑓_𝑣=⎡⎢⎢⎢⎢⎢⎢⎣𝑟^{𝑊𝐶𝑘+1}𝑞^{𝑊𝐶𝑘+1}𝑣^𝑊𝑘+1𝜔^𝑊𝑘+1⎤⎥⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎢⎣𝑞𝑟^𝑘𝑊𝐶+𝑣^𝑊𝑘+𝑉^𝑊𝑘Δ𝑡_𝑘𝑊𝐶×𝑞𝑣𝜔^𝑊𝑘+Ω^𝑊Δ𝑡_𝑊𝑘+𝑉^𝑊𝜔^𝑊𝑘+Ω^𝑊⎤⎥⎥⎥⎥⎥⎥⎦,(2.19) 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: 𝑢=⎡⎢⎢⎣𝑉^𝑊Ω^𝑊⎤⎥⎥⎦=⎡⎢⎢⎣𝛼𝑎^𝑊Δ𝑡_𝑊Δ𝑡⎤⎥⎥⎦.(2.20)

The unknown linear and angular velocity 𝑉^𝑊 and Ω^𝑊 are incorporated into the system by the process noise covariance matrix 𝑈: 𝑈=⎡⎢⎢⎣𝜎^𝑉Δ𝑡²𝐼_3×30_3×30_3×3𝜎_ΩΔ𝑡²𝐼_3×3⎤⎥⎥⎦.(2.21)

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: 𝑥̂_𝑘+1=⎡⎢⎢⎢⎢⎢⎢⎣𝑓^𝑣𝑦₁𝑥̂̂_𝑣𝑦⋮_𝑛̂𝑃⎤⎥⎥⎥⎥⎥⎥⎦,(2.22)_𝑘+1=∇𝐹_𝑥𝑃_𝑘𝐹^𝑇𝑥+∇𝐹_𝑢𝒬∇𝐹^𝑇𝑢,(2.23) where 𝑄 and the Jacobians ∇𝐹_𝑥 and ∇𝐹_𝑢 are defined as follows: ∇𝐹_𝑥=⎡⎢⎢⎢⎣𝜕𝜕𝑓𝑥̂^𝑣_𝑣0_13×𝑛0_𝑛×13𝐼_𝑛×𝑛⎤⎥⎥⎥⎦,∇𝐹_𝑢=⎡⎢⎢⎣𝜕𝜕𝑓𝑢^𝑣0^13×𝑛0_𝑛×60_𝑛×𝑛⎤⎥⎥⎦,𝑄=⎡⎢⎢⎣𝑈0_6×𝑛0_𝑛×60_𝑛×𝑛⎤⎥⎥⎦.(2.24)𝜕𝑓_𝑣/𝜕𝑥̂_𝑣 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 𝑦_𝑖̂: ⎡⎢⎢⎣𝑢_𝑑𝑣_𝑑⎤⎥⎥⎦=ℎ_𝑖𝑥̂_𝑣,𝑦_𝑖̂,(2.25)ℎ_𝑖(𝑥̂_𝑣,𝑦_𝑖̂) 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: ℎ^𝑐=⎡⎢⎢⎢⎢⎣ℎℎ^𝑥_𝑦ℎ_𝑧⎤⎥⎥⎥⎥⎦=𝑅_𝐶𝑊⎛⎜⎜⎜⎜⎝⎡⎢⎢⎢⎢⎣𝑥_𝑖𝑦_𝑖𝑧_𝑖⎤⎥⎥⎥⎥⎦+1𝜌_𝑖𝑚𝜃_𝑖,𝜙_𝑖−𝑟^𝑊𝐶⎞⎟⎟⎟⎟⎠.(2.26)ℎ^𝐶 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: ⎡⎢⎢⎣𝑣𝑢⎤⎥⎥⎦=⎡⎢⎢⎢⎣ℎℎ^𝑥_𝑧ℎ_𝑦ℎ_𝑧⎤⎥⎥⎥⎦.(2.27) The camera calibration model is applied to produce the undistorted pixel coordinates 𝑢_𝑢 and 𝑣_𝑢: ⎡⎢⎢⎣𝑢_𝑢𝑣_𝑢⎤⎥⎥⎦=⎡⎢⎢⎣𝑣𝑢₀−𝑓𝑢₀−𝑓𝑣⎤⎥⎥⎦,(2.28) 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: ⎡⎢⎢⎣𝑢_𝑑𝑣_𝑑⎤⎥⎥⎦=⎡⎢⎢⎢⎢⎣𝑢^𝑢−𝑢₀√1+2𝐾₁𝑟²+𝑢₀𝑣_𝑢−𝑣₀√1+2𝐾₁𝑟²+𝑣₀⎤⎥⎥⎥⎥⎦,𝑟=𝑢_𝑢−𝑢₀²+𝑣_𝑢−𝑣₀².(2.29)

2.5. Features Matching and Filter Update Equations

If the feature 𝑦_𝑖̂ is predicted to be appeared in the image (0<𝑢_𝑑< image height & 0<𝑣_𝑑< 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 𝑆_𝑖: 𝑆_𝑖=∇𝐻_𝑖𝑃_𝑘+1∇𝐻^𝑇𝑖+𝑅_𝑢𝑣.(2.30)

Measurement noise is incorporated into the system by the noise covariance matrix 𝑅_𝑢𝑣: 𝑅_𝑢𝑣=⎡⎢⎢⎣𝜎^2𝑢0𝜎0_2𝑣⎤⎥⎥⎦.(2.31)

Variances 𝜎^2𝑢 and 𝜎^2𝑣 are defined in pixel units. 𝑃_𝑘+1is the prior state covariance estimated by (2.23). ∇𝐻_𝑖 is the Jacobian derived from the measurement model ℎ_𝑖: ∇𝐻_𝑖=𝜕ℎ^𝑖𝜕𝑥̂_𝑣⋯0_2×6⋯𝜕ℎ^𝑖𝜕𝑦_𝑖̂⋯0_2×6⋯,(2.32)𝜕ℎ_𝑖/𝜕𝑥̂_𝑣 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): ⎡⎢⎢⎣𝑠_𝑥𝑠_𝑦⎤⎥⎥⎦=⎡⎢⎢⎣2𝑛√𝑆_𝑖(1,1)2𝑛√𝑆_𝑖(2,2)⎤⎥⎥⎦,(2.33) where 𝑛 is the number of standard deviations for the desired region of search.

Figure 4 

An active search is used to maximize the computational speed and to reduce the mismatch likelihood in the data association process. Feature search is constrained to regions around the predicted ℎ𝑖. The regions are defined by the innovation covariance 𝑆𝑖 as in (2.30).

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: 𝑃𝑥̂_𝑘=𝑥̂_𝑘+1+𝑊𝑔,_𝑘=𝑃_𝑘+1−𝑊𝑆_𝑖𝑊^𝑇,(2.34) where the innovation 𝑔 is as follows: 𝑔=𝑧_𝑖−ℎ_𝑖,(2.35) and 𝑊 is the Kalman gain: 𝑊=𝑃_𝑘+1∇𝐻^𝑇𝑖𝑆_𝑖−1.(2.36)

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 𝑦̂_new 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.

Figure 5 

New features will be added to the map if the number of predicted features 𝑦𝑖̂ to be appearing in the image is lower than a threshold. First, random feature-free areas are detected in the image (green rectangle). Then, a saliency operator is applied in order to detect new candidate points 𝜆𝑖.

When a candidate point 𝜆_𝑖 is detected for the first time, some data are stored: 𝜆_𝑖=𝑟_𝜆,𝜎_𝑟,𝑞_𝜆,𝜎_𝑞,𝑢₁,𝑣₁,(2.37) where 𝑟_𝜆 is taken from the current camera optical center position 𝑟^𝑊𝐶: 𝑟_𝜆=𝑥₁,𝑦₁,𝑧₁=𝑥_𝑣,𝑦_𝑣,𝑧_𝑣,(2.38)𝜎_𝑟 represents the associated variances of 𝑟^𝑊𝐶, taken from the state covariance matrix 𝑃: 𝜎_𝑟=𝜎_𝑥1,𝜎_𝑦1,𝜎_𝑧1=𝑃_(1,1),𝑃_(2,2),𝑃_(3,3),(2.39)𝑞_𝜆 is taken from the quaternion 𝑞^𝑊𝐶 representing the current camera orientation: 𝑞_𝜆=𝑞^𝑊𝐶,(2.40)𝜎_𝑞 represents the associated variances of 𝑞^𝑊𝐶, taken from the state covariance matrix 𝑃: 𝜎_𝑞=𝜎_1𝑞1,𝜎_1𝑞2,𝜎_1𝑞3,𝜎_1𝑞4=𝑃_(4,4),𝑃_(5,5),𝑃_(6,6),𝑃_(7,7),(2.41) 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 𝑦̂_new. 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 𝑦̂_new when a minimum threshold of parallax 𝛼_min 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 𝑧_𝑖.

Figure 6 

Representation for the feature initialization process.

For each candidate point 𝜆_𝑖, when a new measurement 𝑧_𝑖 is available then parallax angle 𝛼 is estimated by the following: 𝛼=𝜋−(𝛽+𝛾).(2.42) 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: 𝛽=cos⁻¹ℎ¹⋅𝑏₁‖‖ℎ₁‖‖‖‖𝑏₁‖‖,(2.43) where the directional projection ray vector ℎ₁=[ℎ_1𝑥ℎ_1𝑦ℎ_1𝑧], 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 𝜆_𝑖. ℎ₁=𝑅^𝑊𝐶𝑞_𝜆ℎ^𝐶1𝑢_1𝑢,𝑣_1𝑢.(2.44)𝑅^𝑊𝐶(𝑞_𝜆) 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.

ℎ^𝐶1(𝑢_1𝑢,𝑣_1𝑢)=[ℎ_1𝑥ℎ_1𝑦ℎ_1𝑧] 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: ℎ^𝐶1𝑢_1𝑢,𝑣_1𝑢=𝑢⁰−𝑢_1𝑢𝑓𝑣⁰−𝑣_1𝑢𝑓1.(2.45) The undistorted pixel coordinates 𝑢_1𝑢 and 𝑣_1𝑢 are obtained from 𝑢₁,𝑣₁ applying the inverse of the distortion model in (2.29): ⎡⎢⎢⎣𝑢_1𝑢𝑣_1𝑢⎤⎥⎥⎦=⎡⎢⎢⎢⎢⎣𝑢¹−𝑢₀√1−2𝐾₁𝑟²+𝑢₀𝑣₁−𝑣₀√1−2𝐾₁𝑟²+𝑣₀⎤⎥⎥⎥⎥⎦,𝑟=𝑢₁−𝑢₀²+𝑣₁−𝑣₀².(2.46)𝑏₁=[𝑏_1𝑥𝑏_1𝑦𝑏_1𝑧] 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 𝑥̂_𝑣: 𝑏₁=𝑥_𝑣−𝑥₁𝑦_𝑣−𝑦₁𝑧_𝑣−𝑧₁.(2.47) The angle 𝛾 is determined in a similar way as 𝛽 but using instead the directional projection ray vector ℎ₂ and the vector 𝑏₂: 𝛾=cos⁻¹ℎ²⋅𝑏₂‖‖ℎ₂‖‖‖‖𝑏₂‖‖.(2.48)

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 𝑢_𝑢,𝑣_𝑢: ℎ₂=𝑅^𝑊𝐶𝑞^𝑊𝐶ℎ^𝐶2𝑢_𝑢,𝑣_𝑢,(2.49)𝑅^𝑊𝐶(𝑞^𝑊𝐶) 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.

ℎ^𝐶2(𝑢_𝑢,𝑣_𝑢)=[ℎ_2𝑥ℎ_2𝑦ℎ_2𝑧] 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: ℎ^𝐶2𝑢_𝑢,𝑣_𝑢=𝑢⁰−𝑢_𝑢𝑓𝑣⁰−𝑣_𝑢𝑓1.(2.50) The undistorted coordinates pixel 𝑢_𝑢 and 𝑣_𝑢 are obtained from the current pixel coordinates 𝑢, 𝑣 applying the inverse of the distortion model in (2.29): ⎡⎢⎢⎣𝑢_𝑢𝑣_𝑢⎤⎥⎥⎦=⎡⎢⎢⎢⎢⎣𝑢−𝑢⁰√1−2𝐾₁𝑟²+𝑢₀𝑣−𝑣₀√1−2𝐾₁𝑟²+𝑣₀⎤⎥⎥⎥⎥⎦,𝑟=𝑢−𝑢₀²+𝑣−𝑣₀².(2.51)𝑏₂=[𝑏_2𝑥𝑏_2𝑦𝑏_2𝑧] is similar to 𝑏₁ but pointing to the opposite direction: 𝑏₂=𝑥₁−𝑥_𝑣𝑦₁−𝑦_𝑣𝑧₁−𝑧_𝑣.(2.52) The base-line 𝑏 is the module of 𝑏₁ or 𝑏₂: 𝑏=‖‖𝑏₁‖‖=‖‖𝑏₂‖‖.(2.53) If 𝛼>𝛼_min then 𝜆_𝑖 is initialized as a new feature map 𝑦̂_new. The feature 𝑦̂_new is made up of: 𝑦̂_new=𝑔𝑥̂,𝜆_𝑖,𝑧_𝑖=𝑥_𝑖𝑦_𝑖𝑧_𝑖𝜃_𝑖𝜙_𝑖𝜌_𝑖^𝑇.(2.54) The first three elements of 𝑦̂_new are obtained directly from the current camera optical center position 𝑟^𝑊𝐶=[𝑥_𝑣,𝑦_𝑣,𝑧_𝑣] taken from 𝑥̂_𝑣: ⎡⎢⎢⎢⎢⎣𝑥_𝑖𝑦_𝑖𝑧_𝑖⎤⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎣𝑥_𝑣𝑦_𝑣𝑧_𝑣⎡⎢⎢⎣𝜃_𝑖⎤⎥⎥⎥⎥⎦,𝜙_𝑖⎤⎥⎥⎦=⎡⎢⎢⎢⎣atan2−ℎ_2𝑦,ℎ^22𝑥+ℎ^22𝑧atan2ℎ_2𝑥,ℎ_2𝑧⎤⎥⎥⎥⎦,(2.55) where [ℎ_2𝑥ℎ_2𝑦ℎ_2𝑧] is computed using (2.49).

The inverse depth 𝜌_𝑖 is derived from the following: 𝜌_𝑖=𝑏ssinin((𝛼𝛽)).(2.56) The new system state 𝑥̂_new is made up simply adding the new feature 𝑦̂_new to the final part of the vector state 𝑥̂. If 𝑥̂=[𝑥̂_𝑣𝑦₁̂𝑦₂̂]^𝑇 then 𝑥̂_new is composed by the following: 𝑥̂=⎡⎢⎢⎢⎢⎣𝑥𝑦₁̂^𝑣̂𝑦₂̂⎤⎥⎥⎥⎥⎦,𝑥̂_new=⎡⎢⎢⎢⎢⎢⎢⎣𝑥̂^𝑣𝑦₁̂𝑦₂̂𝑦̂_new⎤⎥⎥⎥⎥⎥⎥⎦.(2.57) The new state covariance matrix 𝑃_new is derived from the error diagonal measurement covariance matrix 𝑅_𝑗 and the current state covariance matrix 𝑃: 𝑅_𝑗=diag𝜎^2𝑢𝜎^2𝑣𝜎^2𝑢𝜎^2𝑣𝜎_𝑥1𝜎_𝑦1𝜎_𝑧1𝜎_1𝑞1𝜎_1𝑞2𝜎_1𝑞3𝜎_1𝑞4,(2.58) where 𝑅_𝑗 is composed by the measurement error variances 𝜎^2𝑢 and 𝜎^2𝑣 (defined in pixel units), and the variances stored with the candidate point 𝜆_𝑖.

The new state covariance matrix 𝑃_new after the initialization process is given by the following: 𝑃_new=∇𝑌⎡⎢⎢⎣𝑃^𝑘0𝑅0_𝑗⎤⎥⎥⎦∇𝑌_𝑇.(2.59) The Jacobian ∇𝑌 is composed of the following: ∇𝑌=⎡⎢⎢⎢⎣𝐼^𝑛×𝑛𝜕𝑔0𝜕𝑥̂_𝑣,0_6×6,…,0_6×6,𝜕𝑔𝜕𝑅_𝑗⎤⎥⎥⎥⎦,(2.60) 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., 𝛽≈0^∘). 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 𝛽<20^∘).

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

The value of 𝑏_min can be heuristically determined depending on the particularities of the application. In this case, for initializing a candidate point 𝜆_𝑖 as a new feature 𝑦̂_new, where 𝑏>𝑏_min but 𝛼<𝛼_min, 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 𝜌_ini, 𝜎_𝜌(ini) depending on 𝑏_min and 𝛼_min: 𝜌_ini<2sin𝛼^min/2𝑏_min𝜎_𝜌(ini)=𝜌^min4.(2.61)

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:⎛⎜⎜⎜⎜⎜⎜⎝𝑥_𝑣𝑦₁𝑦₂𝑦₃⎞⎟⎟⎟⎟⎟⎟⎠→⎛⎜⎜⎜⎜⎝𝑥_𝑣𝑦₁𝑦₃⎞⎟⎟⎟⎟⎠⎡⎢⎢⎢⎢⎢⎢⎣𝑃^𝑥𝑥𝑃_𝑥𝑦1𝑃_𝑥𝑦2𝑃_𝑥𝑦3𝑃_𝑦1𝑥𝑃_𝑦1𝑦1𝑃_𝑦1𝑦2𝑃_𝑦1𝑦3𝑃_𝑦2𝑥𝑃_𝑦2𝑦2𝑃_𝑦2𝑦2𝑃_𝑦2𝑦3𝑃_𝑦3𝑥𝑃_𝑦3𝑦1𝑃_𝑦3𝑦2𝑃_𝑦3𝑦3⎤⎥⎥⎥⎥⎥⎥⎦→⎡⎢⎢⎢⎢⎣𝑃_𝑥𝑥𝑃_𝑥𝑦1𝑃_𝑥𝑦3𝑃_𝑦1𝑥𝑃_𝑦1𝑦1𝑃_𝑦1𝑦3𝑃_𝑦3𝑥𝑃_𝑦3𝑦1𝑃_𝑦3𝑦3⎤⎥⎥⎥⎥⎦.(2.62)

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, 𝜎_𝑉=4 m/s², 𝜎_Ω=4 m/s², noise variances 𝜎_𝑢=𝜎_𝑣=1 pixel, minimum base-line 𝑏_min=15 cm and minimum parallax angle 𝛼_min=5^∘.

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, 3𝜎 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 7 

Input images (left column), camera trajectory (𝑋-𝑌 view; central column), and map estimations (𝑋-𝑍 view; right column) for a video sequence of 790 frames acquired in a desktop environment. Frames 1, 111, 203, 533, and 688 are displayed.

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.

Figure 8 

Composite top-view picture for a desktop environment (left plot). Corresponding camera trajectory and map estimations in 𝑋-𝑍 view after 790 frames (right plot). Observe that both, camera trajectory and scene structure, have been recovered in a satisfactory manner.

Figure 9 

Input images (left column), camera trajectory (𝑋-𝑌 view; central column) and map estimations (𝑋-𝑍 view; right column) for a video sequence containing about 1750 frames recorded in a laboratory environment. Frames 3, 306, 565, 777, and 1635 are displayed.

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.

Figure 10 

3D view corresponding to the camera trajectory and map estimates after 1750 frames for the experiment in a laboratory environment shown in Figure 8.

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: 𝑞(𝜔)=cos‖𝜔‖2sin‖𝜔2‖‖𝜔𝜔‖^𝑇.(A.1) A quaternion 𝑞 can be computed form a rotation matrix 𝑅 by the following: 𝑞𝑅^𝑏𝑛=⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣𝑞¹𝑅^𝑏𝑛(3,2)−𝑅^𝑏𝑛(2,3)4𝑞₁𝑅^𝑏𝑛(1,3)−𝑅^𝑏𝑛(3,1)4𝑞₁𝑅^𝑏𝑛(2,1)−𝑅^𝑏𝑛(1,2)4𝑞₁⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦,𝑞₁=1+𝑅^𝑏𝑛(1,1)+𝑅^𝑏𝑛(2,2)+𝑅^𝑏𝑛(3,3).(A.2) A rotation matrix 𝑅 can be computed from a quaternion 𝑞 by the following: 𝑅=⎡⎢⎢⎢⎢⎣𝑞²¹2+𝑞₂𝑞²²−𝑞²³−𝑞²⁴2𝑞₂𝑞₃−𝑞₁𝑞₄2𝑞₁𝑞₃+𝑞₂𝑞₄2𝑞₂𝑞₃+𝑞₁𝑞₄𝑞²¹+𝑞²²−𝑞²³−𝑞²⁴2𝑞₃𝑞₄−𝑞₁𝑞₂𝑞₄−𝑞₁𝑞₃2𝑞₁𝑞₂+𝑞₃𝑞₄𝑞²¹+𝑞²²−𝑞²³−𝑞²⁴⎤⎥⎥⎥⎥⎦.(A.3)

Acknowledgments

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