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

Authors: Rodrigo Munguía; Antoni Grau
Journal: Mathematical Problems in Engineering
 (2012)

---

## 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 ina 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.


---

## Body

## 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 n frames; those n 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 instantt is added to the map in a subsequent time t+k. 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 t 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 Figure1 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 statex^ consists of:
(2.1)x^=[x^v,y^1,y^2,…,y^n]T,
where x^v represents the state of a free robot camera moving in any direction in ℝ3×SO(3), and y^i represents a feature point i in the 3D scene. At the same time, x^v is composed of:
(2.2)x^v=[rWCqWCvWωW]T,
where
(2.3)rWC=[xvyvzv]T,
represents the camera optical center position in Cartesian coordinates, and:
(2.4)qWC=[q1q2q3q4]T
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)vW=[vxvyvz]T,ωW=[ωxωyωz]T,
denote linear and angular velocities, respectively. The superscripts W and WC denote magnitudes expressed in the world reference, and in the camera reference respectively.A featurey^i is composed of the following 6-dimension state vector:
(2.6)y^i=[xiyiziθiϕiρi]T,
which models the 3D point located at:
(2.7)[xiyizi]+1ρim(θi,ϕi),
where xi, yi, zi corresponds to the optical center coordinates of the camera when the feature was observed for the very first time; and θi,ϕi, represent the azimuth and the elevation respectively (respect to the world reference W) for the directional unitary vector m(θi,ϕi):
(2.8)m(θi,ϕi)=[cosϕsinθ-sinϕcosϕcosθ]TThe point depthri is coded by its inverse value, ρi = 1/ri, 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 ofn-point (PnP) problem [30]. The PnP problem is to find the position and orientation of a camera with respect to a scene object from n 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)f (focal length); (ii) i0, j0 (displacement to the image center), and (iii) k1,…,kn (radial lens distortion).The problem consists in estimating two extrinsic camera parameters:RCW (world to camera rotation matrix for camera orientation) and t (translation vector for the position of the camera center), given 4 coplanar points with spatial coordinates (xi, yi, 0), with i in [1..4], and their corresponding 4 undistorted image coordinates (i,j), as shown in Figure 3.Figure 3
Perspective of 4 coplanar points (P4P) problem.In order to estimateRCW and t, the method is based in the approach proposed in [31]. The following system of linear equations is formed with the vector b as an unknown parameter:
(2.9)[x1fy1f00-i1x1-i1y1f000x1fy1f-j1x1-j1y10fx2fy2f00-i2x2-i2y2f000x2fy2f-j2x2-j2y20fx3fy3f00-i3x3-i3y3f000x3fy3f-j3x3-j3y30fx4fy4f00-i4x4-i4y4f000x4fy4f-j4x4-j4y40f]b=[i1j1i2j2i3j3i4j4],
where
(2.10)b=[r11t3r12t3r21t3r22t3r31t3r32t3t1t3t2t3]T.
The linear system in (2.9) is solved for b=[b1b2b3b4b5b6b7b8]T. Then t3 is obtained as follows:
(2.11)t3=f2b12+b32+f2b52.
The extrinsic parameters RCW and t are obtained using the following equations:
(2.12)RCW=[t3b1t3b2(R21R32-R31R22)t3b3t3b4(R31R12-R11R32)t3b5t3b6(R11R22-R21R12)],(2.13)t=[t3b7t3b8t3]T.In (2.12) the third column of matrix RCW 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 statex^ini is formed by the camera-state x^v, and the four initial features used for estimating the extrinsic camera parameters:
(2.14)x^ini=[riniWCqiniWCviniWωiniWy^1y^2y^3y^4]T,
where
(2.15)riniWC=t,qiniWC=q((RCW)T),viniW=[03×1],ωiniW=[03×1],riniWR has the same value that the extrinsic parameter t calculated by (2.13). qiniWC is estimated using the rotation matrix to quaternion transformation q(R), described by (A.2) in the Appendix, from the transpose of the rotation matrix (RCW)T obtained by (2.12).Each initial featurey^i with i in [1..4] is composed by:
(2.16)y^i=[riniWCatan2(g1,g3)atan2(-g2,g12+g32)1∥riniWC∥]T,
where
(2.17)[g1g2g3]=[xiyi0]-riniWC,atan2 is a two-argument function that computes the arctangent of y/x for given values of y and x, within the range [-π, π]. If certain confidence is assumed in the estimation process for the initial system state x^ini, then the initial covariance matrix of the system Pini can be filled with zeros or ε, being this value an arbitrary very small positive value:
(2.18)Pini=[ε37×37].


### 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]:
(2.19)fv=[rk+1WCqk+1WCvk+1Wωk+1W]=[rkWC+(vkW+VkW)ΔtqkWC×q((ωkW+ΩW)Δt)vkW+VWωkW+ΩW],
being q((ωkW+ΩW)Δt) the quaternion computed from the rotation vector (ωkW+ΩW)Δt. 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 k, that the changes in linear and angular velocity vW and ωW are produced by an input u of unknown linear and angular velocity VW and ΩW, with linear and angular acceleration with zero-mean and known Gaussian process covariance, aW and αW, respectively:
(2.20)u=[VWΩW]=[aWΔtαWΔt].The unknown linear and angular velocityVW and ΩW are incorporated into the system by the process noise covariance matrix U:
(2.21)U=[(σVΔt)2I3×303×303×3(σΩΔt)2I3×3].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:(2.22)x^k+1=[fv(x^v)y^1⋮y^n],(2.23)Pk+1=∇FxPkFxT+∇Fu𝒬∇FuT,
where Q and the Jacobians ∇Fx and ∇Fu are defined as follows:
(2.24)∇Fx=[∂fv∂x^v013×n0n×13In×n],∇Fu=[∂fv∂u013×n0n×60n×n],Q=[U06×n0n×60n×n].∂fv/∂x^v are the derivatives of the equations of the prediction model fv with respect to the camera state x^v. ∂fv/∂u are the derivatives of the equations of the prediction model fv with respect to the parameters of the covariance matrix U. In this case n corresponds to the dimension of the features y^i in the system state x^v.


### 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 modelhi to forecast the distorted pixel coordinates ud and vd for each feature y^i:
(2.25)[udvd]=hi(x^v,y^i),hi(x^v,y^i) is derived using the following procedure: the model observation of any point y^i from a camera location defines a ray expressed in the camera frame as follows, see Figure 2:
(2.26)hc=[hxhyhz]=RCW([xiyizi]+1ρim(θi,ϕi)-rWC).hC is observed by the camera through its projection in the image. RCW is the transformation matrix from the global reference frame to the camera reference frame. RCWiscomputed by RCW=(RCW(qWC))T where RWC(qWC) is a rotation matrix depending on the camera orientation quaternion qWC using (A.3) which computes a rotation matrix from a quaternion. The directional unitary vector m(θi,ϕi) is computed with (2.8).The vectorhC is projected to the normalized camera retina by the following:
(2.27)[uv]=[hxhzhyhz].
The camera calibration model is applied to produce the undistorted pixel coordinates uu and vu:
(2.28)[uuvu]=[u0-fuv0-fv],
where u0,v0 is the camera center in pixels, and f is the focal length. Finally, a radial distortion model is applied to obtain the distortion pixel coordinates ud,vd. In [18] the following distortion model is proposed being K1 the distortion coefficient:
(2.29)[udvd]=[uu-u01+2K1r2+u0vu-v01+2K1r2+v0],r=(uu-u0)2+(vu-v0)2.


### 2.5. Features Matching and Filter Update Equations

If the featurey^i is predicted to be appeared in the image (0<ud< image height & 0<vd< 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 hi= [ud,vd]. The elliptical regions are defined by the innovation covariance matrix Si:
(2.30)Si=∇HiPk+1∇HiT+Ruv.Measurement noise is incorporated into the system by the noise covariance matrixRuv:
(2.31)Ruv=[σu200σv2].Variancesσu2 and σv2 are defined in pixel units. Pk+1is the prior state covariance estimated by (2.23). ∇Hi is the Jacobian derived from the measurement model hi:
(2.32)∇Hi=[∂hi∂x^v⋯02×6⋯∂hi∂y^i⋯02×6⋯],∂hi/∂x^v are the derivatives of the equations of the measurement prediction model hi with respect to the camera state x^v. ∂hi/∂y^i are the derivatives of the measurement prediction model hi with respect to the parameters of the feature y^i. Note that ∂hi/∂y^i has only a nonzero value at the location (indexes) of the observed feature y^i.The size of the axissx,sy for the elliptical search region defined by the matrix Si is determined by (see Figure 4):
(2.33)[sxsy]=[2nSi(1,1)2nSi(2,2)],
where n 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 predictedhi. The regions are defined by the innovation covariance Si as in (2.30).When a featurey^i is added to the map, a unique image patch of n-by-n 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 (ud,vd) within the search region defined in (2.33). If the score of a pixel location (ud,vd), 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 (ud,vd) is considered as the current feature measurement zi.If the matching process is successful, then the filter is updated as follows:(2.34)x^k=x^k+1+Wg,Pk=Pk+1-WSiWT,
where the innovation g is as follows:
(2.35)g=zi-hi,
and W is the Kalman gain:
(2.36)W=Pk+1∇HiTSi-1.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 axessx, sy 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 featuresy^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 featuresy^i 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,λi. Only image areas free of both, candidate points λi and mapped features y^i, 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 featuresy^i 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 λi.When a candidate pointλi is detected for the first time, some data are stored:
(2.37)λi=(rλ,σr,qλ,σq,u1,v1),
where rλ is taken from the current camera optical center position rWC:
(2.38)rλ=(x1,y1,z1)=(xv,yv,zv),σr represents the associated variances of rWC, taken from the state covariance matrix P:
(2.39)σr=(σx1,σy1,σz1)=(P(1,1),P(2,2),P(3,3)),qλ is taken from the quaternion qWC representing the current camera orientation:
(2.40)qλ=qWC,σq represents the associated variances of qWC, taken from the state covariance matrix P:
(2.41)σq=(σ1q1,σ1q2,σ1q3,σ1q4)=(P(4,4),P(5,5),P(6,6),P(7,7)),
and u1,v1 are the initial pixel coordinates where the candidate point λi was detected for the first time. In subsequent frames, each candidate point λi is intended to be tracked (this approach is independent of the used tracking method) in order to maintain the pixel coordinates u and v updated for such a candidate point. In practice, not all the candidate points λi can be tracked, nevertheless the method takes advantage of this fact in order to prune weak image features. For each candidate point λi, the tracking process is realized until the point is pruned or initialized in the system as a new feature y^new. In practice for each frame, the candidate points λi could be detected, pruned or considered to be added to the map.A candidate pointλi is added as a new feature y^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-lineb;(ii)
λi and associated data;(iii)
the current camera statex^v and current measurement zi.Figure 6
Representation for the feature initialization process.For each candidate pointλi, when a new measurement zi is available then parallax angle α is estimated by the following:
(2.42)α=π-(β+γ).
The angle β is determined by the directional unitary vector h1 and the vector b1 defining the base-line b in the direction of the camera trajectory by the following:
(2.43)β=cos-1(h1·b1∥h1∥∥b1∥),
where the directional projection ray vector h1=[h1xh1yh1z], expressed in the absolute frame W, 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 λi. 								(2.44)h1=RWC(qλ)h1C(u1u,v1u).RWC(qλ) is the transformation matrix from the camera reference frame to the global reference frame, depending on the stored quaternion qλ; (A.3) is used for computing a rotation matrix from a quaternion.h
1
C
(
u
1
u
,
v
1
u
)
=
[
h
1
x
h
1
y
h
1
z
] is the directional vector, in the camera frame C, pointing from the position when the candidate point λi was observed for the first time to the feature location, and it can be computed by the following:
(2.45)h1C(u1u,v1u)=[u0-u1ufv0-v1uf1].
The undistorted pixel coordinates u1u and v1u are obtained from u1,v1 applying the inverse of the distortion model in (2.29):
(2.46)[u1uv1u]=[u1-u01-2K1r2+u0v1-v01-2K1r2+v0],r=(u1-u0)2+(v1-v0)2.b1=[b1xb1yb1z] is the vector representing the camera base-line b between the camera optical center position [x1y1z1], where the point was observed for the first time, and the current camera optical center rWC=[xvyvzv], taken from the current camera state x^v:
(2.47)b1=[(xv-x1)(yv-y1)(zv-z1)].
The angle γ is determined in a similar way as β but using instead the directional projection ray vector h2 and the vector b2:
(2.48)γ=cos-1(h2·b2∥h2∥∥b2∥).The directional projection ray vectorh2 expressed in the absolute frame W, is computed in a similar way as h1 but using the current camera position x^v and therefore, the current undistorted points coordinates uu,vu:
(2.49)h2=RWC(qWC)h2C(uu,vu),RWC(qWC) is the transformation matrix from the camera reference frame to the global reference frame, depending on the current quaternion qWC. Equation (A.3) is used for computing a rotation matrix from a quaternion.h
2
C
(
u
u
,
v
u
)
=
[
h
2
x
h
2
y
h
2
z
] is the directional vector, in the camera frame C, pointing from the current camera position to the feature location, and it can be computed as follows:
(2.50)h2C(uu,vu)=[u0-uufv0-vuf1].
The undistorted coordinates pixel uu and vu are obtained from the current pixel coordinates u, v applying the inverse of the distortion model in (2.29):
(2.51)[uuvu]=[u-u01-2K1r2+u0v-v01-2K1r2+v0],r=(u-u0)2+(v-v0)2.b2=[b2xb2yb2z] is similar to b1 but pointing to the opposite direction:
(2.52)b2=[(x1-xv)(y1-yv)(z1-zv)].
The base-line b is the module of b1 or b2:
(2.53)b=∥b1∥=∥b2∥.
If α>αmin then λi is initialized as a new feature map y^new. The feature y^new is made up of:
(2.54)y^new=g(x^,λi,zi)=[xiyiziθiϕiρi]T.
The first three elements of y^new are obtained directly from the current camera optical center position rWC=[xv,yv,zv] taken from x^v:
(2.55)[xiyizi]=[xvyvzv],[θiϕi]=[atan2(-h2y,h2x2+h2z2)atan2(h2x,h2z)],
where [h2xh2yh2z] is computed using (2.49).The inverse depthρi is derived from the following:
(2.56)[ρi]=sin(α)bsin(β).
The new system state x^new is made up simply adding the new feature y^new to the final part of the vector state x^. If x^=[x^vy^1y^2]T then x^new is composed by the following:
(2.57)x^=[x^vy^1y^2],x^new=[x^vy^1y^2y^new].
The new state covariance matrix Pnew is derived from the error diagonal measurement covariance matrix Rj and the current state covariance matrix P:
(2.58)Rj=diag[σu2σv2σu2σv2σx1σy1σz1σ1q1σ1q2σ1q3σ1q4],
where Rj is composed by the measurement error variances σu2 and σv2 (defined in pixel units), and the variances stored with the candidate point λi.The new state covariance matrixPnew after the initialization process is given by the following:
(2.59)Pnew=∇Y[Pk00Rj]∇YT.
The Jacobian ∇Y is composed of the following:
(2.60)∇Y=[In×n0∂g∂x^v,06×6,…,06×6,∂g∂Rj],
where I is a n-dimensional identity matrix where n is the dimension of Pk. ∂g/∂x^v are the derivatives of the initializations equations g(x^,λi,zi) with respect to the camera state x^v. Note that the derivatives with respect to the previously mapped features are filled with zeros. ∂g/∂Rj are the derivatives of the initializations equations g(x^,λi,zi) with respect to the parameters of the covariance matrix Rj.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 locationrWC, although this kind of features could provide useful information for estimating camera orientation qWC.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-linebmin can be used as an additional threshold for considering a candidate point λi to be initialized as a new feature y^new. In this case, a candidate point λi is considered to be distant from the camera when b>bmin 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 ofbmin can be heuristically determined depending on the particularities of the application. In this case, for initializing a candidate point λi as a new feature y^new, where b>bmin 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 bmin and αmin:
(2.61)ρini<2sin(αmin/2)bminσρ(ini)=ρmin4.


### 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 featurey2 will be the following:(2.62)(xvy1y2y3)→(xvy1y3)[PxxPxy1Pxy2Pxy3Py1xPy1y1Py1y2Py1y3Py2xPy2y2Py2y2Py2y3Py3xPy3y1Py3y2Py3y3]→[PxxPxy1Pxy3Py1xPy1y1Py1y3Py3xPy3y1Py3y3].


## 2.1. System Parameterization

The complete system statex^ consists of:
(2.1)x^=[x^v,y^1,y^2,…,y^n]T,
where x^v represents the state of a free robot camera moving in any direction in ℝ3×SO(3), and y^i represents a feature point i in the 3D scene. At the same time, x^v is composed of:
(2.2)x^v=[rWCqWCvWωW]T,
where
(2.3)rWC=[xvyvzv]T,
represents the camera optical center position in Cartesian coordinates, and:
(2.4)qWC=[q1q2q3q4]T
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)vW=[vxvyvz]T,ωW=[ωxωyωz]T,
denote linear and angular velocities, respectively. The superscripts W and WC denote magnitudes expressed in the world reference, and in the camera reference respectively.A featurey^i is composed of the following 6-dimension state vector:
(2.6)y^i=[xiyiziθiϕiρi]T,
which models the 3D point located at:
(2.7)[xiyizi]+1ρim(θi,ϕi),
where xi, yi, zi corresponds to the optical center coordinates of the camera when the feature was observed for the very first time; and θi,ϕi, represent the azimuth and the elevation respectively (respect to the world reference W) for the directional unitary vector m(θi,ϕi):
(2.8)m(θi,ϕi)=[cosϕsinθ-sinϕcosϕcosθ]TThe point depthri is coded by its inverse value, ρi = 1/ri, 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 ofn-point (PnP) problem [30]. The PnP problem is to find the position and orientation of a camera with respect to a scene object from n 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)f (focal length); (ii) i0, j0 (displacement to the image center), and (iii) k1,…,kn (radial lens distortion).The problem consists in estimating two extrinsic camera parameters:RCW (world to camera rotation matrix for camera orientation) and t (translation vector for the position of the camera center), given 4 coplanar points with spatial coordinates (xi, yi, 0), with i in [1..4], and their corresponding 4 undistorted image coordinates (i,j), as shown in Figure 3.Figure 3
Perspective of 4 coplanar points (P4P) problem.In order to estimateRCW and t, the method is based in the approach proposed in [31]. The following system of linear equations is formed with the vector b as an unknown parameter:
(2.9)[x1fy1f00-i1x1-i1y1f000x1fy1f-j1x1-j1y10fx2fy2f00-i2x2-i2y2f000x2fy2f-j2x2-j2y20fx3fy3f00-i3x3-i3y3f000x3fy3f-j3x3-j3y30fx4fy4f00-i4x4-i4y4f000x4fy4f-j4x4-j4y40f]b=[i1j1i2j2i3j3i4j4],
where
(2.10)b=[r11t3r12t3r21t3r22t3r31t3r32t3t1t3t2t3]T.
The linear system in (2.9) is solved for b=[b1b2b3b4b5b6b7b8]T. Then t3 is obtained as follows:
(2.11)t3=f2b12+b32+f2b52.
The extrinsic parameters RCW and t are obtained using the following equations:
(2.12)RCW=[t3b1t3b2(R21R32-R31R22)t3b3t3b4(R31R12-R11R32)t3b5t3b6(R11R22-R21R12)],(2.13)t=[t3b7t3b8t3]T.In (2.12) the third column of matrix RCW 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 statex^ini is formed by the camera-state x^v, and the four initial features used for estimating the extrinsic camera parameters:
(2.14)x^ini=[riniWCqiniWCviniWωiniWy^1y^2y^3y^4]T,
where
(2.15)riniWC=t,qiniWC=q((RCW)T),viniW=[03×1],ωiniW=[03×1],riniWR has the same value that the extrinsic parameter t calculated by (2.13). qiniWC is estimated using the rotation matrix to quaternion transformation q(R), described by (A.2) in the Appendix, from the transpose of the rotation matrix (RCW)T obtained by (2.12).Each initial featurey^i with i in [1..4] is composed by:
(2.16)y^i=[riniWCatan2(g1,g3)atan2(-g2,g12+g32)1∥riniWC∥]T,
where
(2.17)[g1g2g3]=[xiyi0]-riniWC,atan2 is a two-argument function that computes the arctangent of y/x for given values of y and x, within the range [-π, π]. If certain confidence is assumed in the estimation process for the initial system state x^ini, then the initial covariance matrix of the system Pini can be filled with zeros or ε, being this value an arbitrary very small positive value:
(2.18)Pini=[ε37×37].


## 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]:
(2.19)fv=[rk+1WCqk+1WCvk+1Wωk+1W]=[rkWC+(vkW+VkW)ΔtqkWC×q((ωkW+ΩW)Δt)vkW+VWωkW+ΩW],
being q((ωkW+ΩW)Δt) the quaternion computed from the rotation vector (ωkW+ΩW)Δt. 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 k, that the changes in linear and angular velocity vW and ωW are produced by an input u of unknown linear and angular velocity VW and ΩW, with linear and angular acceleration with zero-mean and known Gaussian process covariance, aW and αW, respectively:
(2.20)u=[VWΩW]=[aWΔtαWΔt].The unknown linear and angular velocityVW and ΩW are incorporated into the system by the process noise covariance matrix U:
(2.21)U=[(σVΔt)2I3×303×303×3(σΩΔt)2I3×3].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:(2.22)x^k+1=[fv(x^v)y^1⋮y^n],(2.23)Pk+1=∇FxPkFxT+∇Fu𝒬∇FuT,
where Q and the Jacobians ∇Fx and ∇Fu are defined as follows:
(2.24)∇Fx=[∂fv∂x^v013×n0n×13In×n],∇Fu=[∂fv∂u013×n0n×60n×n],Q=[U06×n0n×60n×n].∂fv/∂x^v are the derivatives of the equations of the prediction model fv with respect to the camera state x^v. ∂fv/∂u are the derivatives of the equations of the prediction model fv with respect to the parameters of the covariance matrix U. In this case n corresponds to the dimension of the features y^i in the system state x^v.


## 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 modelhi to forecast the distorted pixel coordinates ud and vd for each feature y^i:
(2.25)[udvd]=hi(x^v,y^i),hi(x^v,y^i) is derived using the following procedure: the model observation of any point y^i from a camera location defines a ray expressed in the camera frame as follows, see Figure 2:
(2.26)hc=[hxhyhz]=RCW([xiyizi]+1ρim(θi,ϕi)-rWC).hC is observed by the camera through its projection in the image. RCW is the transformation matrix from the global reference frame to the camera reference frame. RCWiscomputed by RCW=(RCW(qWC))T where RWC(qWC) is a rotation matrix depending on the camera orientation quaternion qWC using (A.3) which computes a rotation matrix from a quaternion. The directional unitary vector m(θi,ϕi) is computed with (2.8).The vectorhC is projected to the normalized camera retina by the following:
(2.27)[uv]=[hxhzhyhz].
The camera calibration model is applied to produce the undistorted pixel coordinates uu and vu:
(2.28)[uuvu]=[u0-fuv0-fv],
where u0,v0 is the camera center in pixels, and f is the focal length. Finally, a radial distortion model is applied to obtain the distortion pixel coordinates ud,vd. In [18] the following distortion model is proposed being K1 the distortion coefficient:
(2.29)[udvd]=[uu-u01+2K1r2+u0vu-v01+2K1r2+v0],r=(uu-u0)2+(vu-v0)2.


## 2.5. Features Matching and Filter Update Equations

If the featurey^i is predicted to be appeared in the image (0<ud< image height & 0<vd< 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 hi= [ud,vd]. The elliptical regions are defined by the innovation covariance matrix Si:
(2.30)Si=∇HiPk+1∇HiT+Ruv.Measurement noise is incorporated into the system by the noise covariance matrixRuv:
(2.31)Ruv=[σu200σv2].Variancesσu2 and σv2 are defined in pixel units. Pk+1is the prior state covariance estimated by (2.23). ∇Hi is the Jacobian derived from the measurement model hi:
(2.32)∇Hi=[∂hi∂x^v⋯02×6⋯∂hi∂y^i⋯02×6⋯],∂hi/∂x^v are the derivatives of the equations of the measurement prediction model hi with respect to the camera state x^v. ∂hi/∂y^i are the derivatives of the measurement prediction model hi with respect to the parameters of the feature y^i. Note that ∂hi/∂y^i has only a nonzero value at the location (indexes) of the observed feature y^i.The size of the axissx,sy for the elliptical search region defined by the matrix Si is determined by (see Figure 4):
(2.33)[sxsy]=[2nSi(1,1)2nSi(2,2)],
where n 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 predictedhi. The regions are defined by the innovation covariance Si as in (2.30).When a featurey^i is added to the map, a unique image patch of n-by-n 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 (ud,vd) within the search region defined in (2.33). If the score of a pixel location (ud,vd), 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 (ud,vd) is considered as the current feature measurement zi.If the matching process is successful, then the filter is updated as follows:(2.34)x^k=x^k+1+Wg,Pk=Pk+1-WSiWT,
where the innovation g is as follows:
(2.35)g=zi-hi,
and W is the Kalman gain:
(2.36)W=Pk+1∇HiTSi-1.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 axessx, sy 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 featuresy^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 featuresy^i 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,λi. Only image areas free of both, candidate points λi and mapped features y^i, 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 featuresy^i 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 λi.When a candidate pointλi is detected for the first time, some data are stored:
(2.37)λi=(rλ,σr,qλ,σq,u1,v1),
where rλ is taken from the current camera optical center position rWC:
(2.38)rλ=(x1,y1,z1)=(xv,yv,zv),σr represents the associated variances of rWC, taken from the state covariance matrix P:
(2.39)σr=(σx1,σy1,σz1)=(P(1,1),P(2,2),P(3,3)),qλ is taken from the quaternion qWC representing the current camera orientation:
(2.40)qλ=qWC,σq represents the associated variances of qWC, taken from the state covariance matrix P:
(2.41)σq=(σ1q1,σ1q2,σ1q3,σ1q4)=(P(4,4),P(5,5),P(6,6),P(7,7)),
and u1,v1 are the initial pixel coordinates where the candidate point λi was detected for the first time. In subsequent frames, each candidate point λi is intended to be tracked (this approach is independent of the used tracking method) in order to maintain the pixel coordinates u and v updated for such a candidate point. In practice, not all the candidate points λi can be tracked, nevertheless the method takes advantage of this fact in order to prune weak image features. For each candidate point λi, the tracking process is realized until the point is pruned or initialized in the system as a new feature y^new. In practice for each frame, the candidate points λi could be detected, pruned or considered to be added to the map.A candidate pointλi is added as a new feature y^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-lineb;(ii)
λi and associated data;(iii)
the current camera statex^v and current measurement zi.Figure 6
Representation for the feature initialization process.For each candidate pointλi, when a new measurement zi is available then parallax angle α is estimated by the following:
(2.42)α=π-(β+γ).
The angle β is determined by the directional unitary vector h1 and the vector b1 defining the base-line b in the direction of the camera trajectory by the following:
(2.43)β=cos-1(h1·b1∥h1∥∥b1∥),
where the directional projection ray vector h1=[h1xh1yh1z], expressed in the absolute frame W, 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 λi. 								(2.44)h1=RWC(qλ)h1C(u1u,v1u).RWC(qλ) is the transformation matrix from the camera reference frame to the global reference frame, depending on the stored quaternion qλ; (A.3) is used for computing a rotation matrix from a quaternion.h
1
C
(
u
1
u
,
v
1
u
)
=
[
h
1
x
h
1
y
h
1
z
] is the directional vector, in the camera frame C, pointing from the position when the candidate point λi was observed for the first time to the feature location, and it can be computed by the following:
(2.45)h1C(u1u,v1u)=[u0-u1ufv0-v1uf1].
The undistorted pixel coordinates u1u and v1u are obtained from u1,v1 applying the inverse of the distortion model in (2.29):
(2.46)[u1uv1u]=[u1-u01-2K1r2+u0v1-v01-2K1r2+v0],r=(u1-u0)2+(v1-v0)2.b1=[b1xb1yb1z] is the vector representing the camera base-line b between the camera optical center position [x1y1z1], where the point was observed for the first time, and the current camera optical center rWC=[xvyvzv], taken from the current camera state x^v:
(2.47)b1=[(xv-x1)(yv-y1)(zv-z1)].
The angle γ is determined in a similar way as β but using instead the directional projection ray vector h2 and the vector b2:
(2.48)γ=cos-1(h2·b2∥h2∥∥b2∥).The directional projection ray vectorh2 expressed in the absolute frame W, is computed in a similar way as h1 but using the current camera position x^v and therefore, the current undistorted points coordinates uu,vu:
(2.49)h2=RWC(qWC)h2C(uu,vu),RWC(qWC) is the transformation matrix from the camera reference frame to the global reference frame, depending on the current quaternion qWC. Equation (A.3) is used for computing a rotation matrix from a quaternion.h
2
C
(
u
u
,
v
u
)
=
[
h
2
x
h
2
y
h
2
z
] is the directional vector, in the camera frame C, pointing from the current camera position to the feature location, and it can be computed as follows:
(2.50)h2C(uu,vu)=[u0-uufv0-vuf1].
The undistorted coordinates pixel uu and vu are obtained from the current pixel coordinates u, v applying the inverse of the distortion model in (2.29):
(2.51)[uuvu]=[u-u01-2K1r2+u0v-v01-2K1r2+v0],r=(u-u0)2+(v-v0)2.b2=[b2xb2yb2z] is similar to b1 but pointing to the opposite direction:
(2.52)b2=[(x1-xv)(y1-yv)(z1-zv)].
The base-line b is the module of b1 or b2:
(2.53)b=∥b1∥=∥b2∥.
If α>αmin then λi is initialized as a new feature map y^new. The feature y^new is made up of:
(2.54)y^new=g(x^,λi,zi)=[xiyiziθiϕiρi]T.
The first three elements of y^new are obtained directly from the current camera optical center position rWC=[xv,yv,zv] taken from x^v:
(2.55)[xiyizi]=[xvyvzv],[θiϕi]=[atan2(-h2y,h2x2+h2z2)atan2(h2x,h2z)],
where [h2xh2yh2z] is computed using (2.49).The inverse depthρi is derived from the following:
(2.56)[ρi]=sin(α)bsin(β).
The new system state x^new is made up simply adding the new feature y^new to the final part of the vector state x^. If x^=[x^vy^1y^2]T then x^new is composed by the following:
(2.57)x^=[x^vy^1y^2],x^new=[x^vy^1y^2y^new].
The new state covariance matrix Pnew is derived from the error diagonal measurement covariance matrix Rj and the current state covariance matrix P:
(2.58)Rj=diag[σu2σv2σu2σv2σx1σy1σz1σ1q1σ1q2σ1q3σ1q4],
where Rj is composed by the measurement error variances σu2 and σv2 (defined in pixel units), and the variances stored with the candidate point λi.The new state covariance matrixPnew after the initialization process is given by the following:
(2.59)Pnew=∇Y[Pk00Rj]∇YT.
The Jacobian ∇Y is composed of the following:
(2.60)∇Y=[In×n0∂g∂x^v,06×6,…,06×6,∂g∂Rj],
where I is a n-dimensional identity matrix where n is the dimension of Pk. ∂g/∂x^v are the derivatives of the initializations equations g(x^,λi,zi) with respect to the camera state x^v. Note that the derivatives with respect to the previously mapped features are filled with zeros. ∂g/∂Rj are the derivatives of the initializations equations g(x^,λi,zi) with respect to the parameters of the covariance matrix Rj.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 locationrWC, although this kind of features could provide useful information for estimating camera orientation qWC.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-linebmin can be used as an additional threshold for considering a candidate point λi to be initialized as a new feature y^new. In this case, a candidate point λi is considered to be distant from the camera when b>bmin 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 ofbmin can be heuristically determined depending on the particularities of the application. In this case, for initializing a candidate point λi as a new feature y^new, where b>bmin 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 bmin and αmin:
(2.61)ρini<2sin(αmin/2)bminσρ(ini)=ρmin4.


## 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 featurey2 will be the following:(2.62)(xvy1y2y3)→(xvy1y3)[PxxPxy1Pxy2Pxy3Py1xPy1y1Py1y2Py1y3Py2xPy2y2Py2y2Py2y3Py3xPy3y1Py3y2Py3y3]→[PxxPxy1Pxy3Py1xPy1y1Py1y3Py3xPy3y1Py3y3].


## 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,σV=4 m/s2, σΩ=4 m/s2, noise variances σu=σv=1 pixel, minimum base-line bmin=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.Figure7 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 (X-Y view; central column), and map estimations (X-Z view; right column) for a video sequence of 790 frames acquired in a desktop environment. Frames 1, 111, 203, 533, and 688 are displayed.Figure8 shows a top view of the experimental environment (left plot), and the final camera trajectory and map estimates, in an X-Z 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 inX-Z 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 (X-Y view; central column) and map estimations (X-Z 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. Figure9 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 Figure10 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 Figure8.


## 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.



---

*Source: 676385-2012-09-02.xml*