Despite the inherent potential of using external cameras to localize robots, there are relative few attempts to solve it compared to the approach that consider the camera on-board the robot [4, 5]. However, some examples of robot localization with cameras can be found in the literature, where the robot is equipped with artificial landmarks, either active [6, 7] or passive ones [8, 9]. In other works a model of the robot, either geometrical or of appearance [10, 11], is learnt previously to the tracking task. In [12, 13], the position of static and dynamic objects is obtained by multiple camera fusion inside an occupancy grid. An appearance model is used afterwards to ascertain which object is each robot. Despite the technique used for tracking, the common point of many of the proposals found in the topic comes from the fact that rich knowledge is obtained previously to the tracking, in a supervised task.

In this paper the localization of the robot is obtained in terms of its natural appearance. We propose a full metric and accurate system based on identifying natural features belonging to the geometric structure of the robot. On most natural objects we can find points whose image projection can be tracked in the image plane independently of the position the object occupies and based on local properties found in the image (i.e., lines, corners or color blobs). Those points are considered natural markers, as they serve as reference points in the image plane that can be easily relate with their three-dimensional counterparts. The set of methods focused on tracking natural markers have become a very successful and deeply studied topic in the literature [14, 15], as they represent the basic measurements of most of existing reconstruction methods.

Scene reconstruction from image measurements is a classic and mature discipline in the computer vision field. Among the wide amount of proposals it can be highlighted those grouped under the name “Bundle Adjustment” [16, 17]. Their aim is essentially to estimate, jointly and optimally, the 3D structure of a scene and the camera parameters from a set of images taken under some kind of motion. (i.e., it can be the camera that moves or equally some part of the scene w.r.t. the camera).

In general terms, Bundle Adjustment reconstruction methods are based in iterative optimization methods which try to minimize the image discrepancy between the measured positions of the 3D model and the expected ones using the last iteration solution. The discrepancy balances the contribution of the measurements into the final solution and plays an important role in this paper. Our main contribution is a redefinition of the discrepancy function using a Maximum Likelihood approach which takes into account the statistical distribution of the error. This distribution is especially affected by the odometry errors which are accumulative in long trajectories.

On the other hand, once a geometric model is obtained using a structure-from-motion approach, its pose with respect to a global coordinate origin can be easily retrieved by measuring the projection of the model in the image plane. This problem, commonly known as the Perspective n Point Problem (PnP), has received considerable attention in the literature, where some accurate solutions are found such as [18] or the recent global solution proposed in [19]. In this paper we instead follow a filtering approach, where not only image measurements but also last time pose information and odometry information are used to obtain the pose. This approach, which is based on the use of a Kalman Filter, is much more regular than solving the PnP problem at each time instant and will be described in this paper.

The paper is organized as follows. In Section 2. the notation and mathematical elements used in the paper are described. In Section 3. the description of the initialization algorithms is given. The online Kalman loop is explained in Section 4. and some results in a real environment are shown in Section 5. Conclusions are in Section 6.

The robot’s pose at time k is described by a vector X_k. We suppose that the robot’s motion lie on the plane z = 0 referred from the world coordinate origin O_W (See Figure 2). The pose vector X_k is described by 3 components (x_k, y_k, α_k), corresponding to two position coordinates x_k, y_k and the rotation angle α_k in the z axis. The motion model X_k = g(X_k−1, U_k) defines the relationship between the current pose X_k with respect to the previous time one X_k−1 and the input U_k given by odometry sensors inside the robot. In our proposal the robot used is a differential wheeled platform and thus U_k = (Vl_k, Ω_k)^T, where Vl_k and Ω_k describe the linear and angular speed of the robot’s center of rotation (O_R in Figure 2) respectively. The motion model g is then very simple in function of the discretized linear speed Vl_k and the angular speed Ω_k.

The robot’s geometry is composed of a sparse set of N 3D points 𝒨 = {M¹, ⋯, M^N} referred from the local coordinate origin O_R described by robot’s pose X_k. The points Mⁱ are static in time due to robot’s rigidness, and thus, no temporal subindex is required for them. Function M X k i = t ( X k ,   M i ) uses actual pose X_k to express Mⁱ in the global coordinate origin O_W that X_k is referred to (see Figure 2): (1) M X k i = t ( X k ,   M i ) = R k M i + T kwhere: (2) R k = ( cos ( α k ) sin ( α k ) 0 − sin ( α k ) cos ( α k ) 0 0 0 1 )           T k = ( x k y k 0 )

The augmented vector X k a, which is the state vector of the system, is defined as the concatenation in one column vector of both the pose X_k and the set of static structure points 𝒨: (3) X k a = ( ( X k ) T       ( M 1 ) T       ⋯       ( M N ) T ) T

An augmented motion model g^a is defined as the transition of the state vector: (4) X k a = g a ( X k − 1 a , U k ) = ( ( g ( X k − 1 , U k ) ) T       ( M 1 ) T       ⋯       ( M N ) T ) T

It must be noticed that the motion model g^a leaves the structure points contained in the state vector untouched, as we suppose that the object is rigid.

The whole set of measurements obtained in the image plane of the camera at time k is denoted by Y_k. Each measurement inside Y_k is defined by a two-dimensional vector y k i = ( u k i       v k i ) T, describing the projection of each point inside 𝒨 in the image plane, where i stands for the correspondence with the point Mⁱ. It must be remarked that, in general, Y_k does not contain the image projection of every point in the set M as some of them can be occluded depending on the situation.

The camera is modeled with a zero-skew “pin-hole” model (see [17] for more details), with the following matrix K_c containing the intrinsic parameters: (5) K c = ( f u 0 u 0 0 f v v 0 0 0 1 )

The extrinsic parameters of the camera (i.e., position and orientation of the camera w.r.t. O_W) are described using the rigid transformation R_c, T_c (i.e., rotation matrix and translation vector). The matrix R_c is defined by three Euler angles α_c, β_c, γ_c. The vector T_c can be decomposed into its three spatial coordinates T_x, T_y, T_z. All calibration parameters can be grouped inside vector P: (6) P = ( f u ,   f v ,   u 0 ,   v 0 ,   α c ,   β c ,   γ c ,   T x ,   T y ,   T z ) T

Given a single measurement y k i from the set Y_k, its 2D coordinates can be expressed using the “pin-hole” model and the aforementioned calibration parameters: (7) ( y k i 1 ) = λ k i K c   ( R c M X k i + T c ) = λ k i K c   ( R c R k M i + R c T k + T c )where λ k i represents a projective scale factor that can be obtained so that the third element of the right part of Equation (7) is equal to one. It is important to remark that the projection model, although simple, depends in a non-linear fashion w.r.t. Mⁱ due to the factor λ k i. For compactness the projection model of Equation (7) can be described as the function h: (8) y k i = h ( X k ,   M i , P )

In the same way, the whole vector Y_k can be expressed with the following function: (9) Y k = h a ( X k a , P ) = ( h ( X k , M 1 , P ) T ⋯ h ( X k ,   M N , P ) T ) T

This paper explicitly deals with uncertainties by describing every process as a random variable with a Gaussian distribution whose covariance matrix stands for its uncertainty. The random processes are expressed in bold typography (i.e., X_k is the random process describing the pose and X_k is a realization of it) and each of them are defined by a mean vector and a covariance matrix. Therefore X k a is defined by its mean X ^ k a and covariance ∑ k a (or simplifying 𝒩 ( X ^ k a , ∑ k a )). The following processes are considered in the paper:

Pose X_k = 𝒩(X̂_k, ∑_k) and 3D model M = 𝒩(M̂, ∑_M) processes. Its joint distribution is encoded in X k a = 𝒩 ( X ^ k a , ∑ k a ).

A set of odometry readings, U₁, ⋯, U_K are collected over a set of K consecutive time samples k = 1, ⋯, K, which corresponds to the initialization trajectory performed by the robot. Using a motion model g given initial position X₀, an estimation of the robot’s pose in the whole initialization sequence is obtained as follows: (10) X 1 = g ( X 0 , U 1 ) X 2 = g ( X 1 , U 2 )                   ⋮ X K = g ( X K − 1 , U K )where we recall that X_k and U_k denote Gaussian processes. Using expression (10), and by propagating the statistical processes through function g, the joint distribution p(X₁, ⋯, X_K|X₀) can be obtained. The propagation of statistical processes through non-linear functions is in general a very complex task, as it requires to solve integral equations. However in this paper we will follow a first order approximation of the non-linear functions centered in the mean of the process (see [20] for more details). Using this technique the Gaussian processes X_k and U_k can be iteratively propagated through function g at the cost of being an approximation. This paper shows that despite being an approximation this approach end in a good estimation of the initialization vector.

By denoting as X to the joint vector containing all poses X = ( X 1 T , ⋯ ,   X K T ) T, the joint distribution of all poses, given the initial state X₀, p(X|X₀) is approximated as Gaussian p.d.f. with mean X̂ and covariance matrix ∑_X.

In this step we describe how to collect the position in the image plane of different points from the robot’s structure during the initialization trajectory. We base our method in the work [15] where the SIFT descriptor is introduced. The process used in the initialization is composed of two steps:

Feature-based background subtraction.

We describe the static background of the scene using a single image, from which a set of features, 𝒡 b = { y b 1 , ⋯ ,   y b N b } and its correspondent SIFT descriptors 𝒟 1 = { d 1 1 , ⋯ ,   d 1 N d } are obtained. The sets 𝒟_b and 𝒡_b are our feature-based background model.

Given an input image, namely I_k, we find the sets 𝒟_k and 𝒡_k. We consider that a feature y k i ∈ 𝒡 k, d k i ∈ 𝒟 k belongs to the background if we can find a feature y b i ∈ 𝒡 b, d b j ∈ 𝒟 b such that | y k i − y b j | < R max and | d k i − d b j | < d max. This method although simple shows to be very effective and robust in real environments (See Figure 4).

Using the feature-based algorithm proposed before a set of measurements in the whole trajectory, Y₁, ⋯, Y_K, is obtained, where each vector Y_k contains the projection of N points from robot’s structure at time sample k. The set of N points, M¹, ⋯, M^N, jointly with the initial pose X₀, represent the initialization unknowns. As the robot’s motion does not depend of its structure, the following statistical independence is true: (11) p ( X 1 , ⋯ ,   X K | X 0 ) = p ( X 1 , ⋯ ,   X K | X 0 ,   M 1 , ⋯ ,   M N ) = p ( X 1 , ⋯ ,   X K | X 0 a )

If all trajectories are packed into vector Y_L, the following function put in relation Y_L with the distribution of expression (11): (12) Y L = ( Y 1 ⋮ Y K ) = ( h a ( X 1 a , P ) + V 1 ⋮ h a ( X K a , P ) + V K )where V_k represents the uncertainty in image detection. Using distribution showed in (11), and propagating statistics through function (12), the following likelihood distribution is obtained: (13) p ( Y L | X 0 ,   M 1 , ⋯ ,   M N ) = p ( Y L | X 0 a )The likelihood function (13) is represented by a Gaussian distribution using a first order approximation of (12). It is thus defined by a mean Ŷ_L and a covariance matrix ∑_L: (14) p ( Y L | X 0 a ) = 1 | Σ L | 2 N e 1 2 ( Y L − Y ^ L ) T Σ L − 1 ( Y L − Y ^ L )

The likelihood function (14), parametrized by its covariance matrix ∑_L and its mean Ŷ_L, is dependent of the conditional parameters and unknown values X 0 a.

The “Maximum Likelihood” estimation consists of finding the values for X 0 a that maximize the likelihood function: (15) max X 0 , M 1 , ⋯ , M N   p ( Y L | X 0 ,   M 1 , ⋯ ,   M N )

In its Gaussian approximation and by taking logarithms, it is converted into the following minimization problem: (16) min X 0 , M 1 , ⋯ , M N   ( Y L − Y ^ L ) T Σ L − 1 ( Y L − Y ^ L )where Y_L is the realization of the process (i.e., the set of measurements from the image) and Ŷ_L are the expected measurements given a value of the parameters X₀, M¹, ⋯, M^N.

The configuration of ∑_L is ruled by the expected uncertainty in the measurements and the statistical relation between them. Usually all cross-correlated terms of ∑_L are non-zero, which has an important effect in the sparsity of the Hessian used inside the optimization algorithm and consequently in the computational complexity of the problem (see [21] for more details).

The covariance matrix ∑_L can be approximated assuming either independence between time samples (discarding cross-correlation terms between time indexes) or total independence between time and each measurement (discarding all cross-correlation terms except the 2×2 boxes belonging to a single measurement). In Table 1, the different cost functions are shown depending on the discarded terms of ∑_L. The different approximations of ∑_L have a direct impact in the accuracy and the reconstruction error, and the results will be shown in Section 5.

Intuitively, if ∑_L results to be a identity matrix, the cost function is reduced to a simple image residual minimization extensively used in Bundle Adjustment techniques, where in principle all cost differences y k i − y ^ k i have equal importance: (17) min X 0 a ∑ i = 1 N ∑ k = 1 K i | ( y k i − y ^ k i ) | 2

The result of minimizing Equation (16) instead of the usual (17) show significant improvements in the reconstruction error. In Section 5. a comparison is shown between the initialization accuracy under the different assumptions of the matrix ∑_L, from its diagonal version to the complete correlated form. The minimum of (16) is obtained using the Levenberg–Mardquardt [17] iterative optimization method.

We suppose that during the measurement step there is a low probability of encountering outliers in the features. This argument can be very optimistic in some real configurations where more objects appear in the scene and produce occlusions or false matchings. For those cases, all cost functions presented in this section can be modified to be robust against outliers by using M-Estimators. We refer the reader to [17] for more details.

The use of an iterative optimization method for obtaining the minimum of (16) requires a setup value from which to start iterating. An initial estimation of X 0 a close to its correct value reduces the probability to fall in a local extrema of the cost function.

The method proposed in this paper to give an initial value for X 0 a consists of a non-iterative and exact method, which gives directly an accurate solution for X 0 a in absence of noise in odometry values. This method is based on the assumption that the robot moves in a plane and thus only the angle over its Z axis is needed. As explained below, this assumption allows to solve the problem with a rank deficient Linear Least Squares approximation, which is solved using the Singular Value Decomposition of the system’s matrix and imposing a constraint that ensures a valid rotation. Its development is briefly introduced next.

For a point Mⁱ of robot’s model and at time k of the initialization trajectory, the image measurement y k i results from the following projection: (18) ( y k i 1 ) = λ k i K c ( R c ( R k Δ R 0 M i + T 0 + R 0 T k Δ ) + T c )where the matrix R k Δ and vector T k Δ represent the rotation and position of the robot in the floor plane given by the odometry supposing that X 0 = ( 0       0       0 ) T. The rotation matrix R₀ and the offset T₀ correspond with the initial pose X 0 = ( x 0 1 , x 0 2 , α 0 ) in form of rigid transformation: (19) R 0 = ( cos ( α 0 ) − sin ( α 0 ) 0 sin ( α 0 ) cos ( α 0 ) 0 0 0 1 )           T k = ( x 0 1 x 0 2 0 )where R₀ is a non-linear function of the orientation angle. The expression (18) depends non-linearly of vector X 0 a and so a new parametrization is proposed jointly with a transformation which removes the projective parameter λ k i.

The point Mⁱ is replaced by the rotated M X 0 i = R 0 M i, removing thus the product between unknowns. The orientation angle in the pose is substituted by parameters a = cos(α₀) and b = sin(α₀), with the constraint a² + b² = 1. Using the new parameterization the expression (18) is transformed in the following: (20) ( y k i 1 ) = λ K c ( R c ( R k Δ M X 0 i + T 0 + L T k Δ ( a b 0 ) ) + T c )with L T k Δ: (21) L T k Δ = ( x k 1 , Δ − x k 2 , Δ 0 − x k 2 , Δ x k 1 , Δ 0 0 0 1 )where x k 1 , Δ y x k 2 , Δ are the two first coordinates of T k Δ.

The new unknown vector, which correspond to the new parametrization of X 0 a, is: (22) Φ = ( x 0 1 x 0 2 a b ( M X 0 1 ) T , ⋯ , ( M X 0 N ) T ) T

The expression (20) is decomposed in the following elements: (23) ( u k i v k i ) = y k i           ( U k i V k i S k i ) = K ( R c ( R k Δ M X 0 i + T 0 + L T k Δ ) + T c )where U k i, V k i y S k i are linear in terms of Φ but not in terms of y k i. (24) U k i = L U k i Φ + b U k i             V k i = L V k i Φ + b V k i             S k i = L S k i Φ + b S k i

The projective scale is removed from the transformation by using vector product properties: (25) ( u k i v k i 1 ) = λ k i ( U k i V k i S k i ) → ( u k i v k i 1 ) × ( U k i V k i S k i ) = ( 0 0 0 )where two lineally independent equations are obtained for Φ. (26) { v k i S k i − V k i = 0 − u k i S k i + U k i = 0             → { ( v k i L S k i − L V k i ) Φ = v k i b S k i − b V k i ( − u k i L S k i + L U k i ) Φ = − u k i b S k i + b U k i

Using all measurements inside Y_L, a lineal system of equations is obtained in terms of Φ: (27) A Φ = B             A = ( ( v 1 1 L S 1 1 − L V 1 1 ) ( − u k 1 L S 1 1 + L U 1 1 ) ⋮ ( v K N L S K N − L V K N ) ( − u K N L S K N + L U K N ) )             B = ( v 1 1 b S 1 1 − b V 1 1 − u 1 1 b S 1 1 + b U 1 1 ⋮ v K N b S K N − b V K N − u K N b S K N + b U K N )

It is straightforward to show that system of (27) has a single-parameter family of solutions. If Φ₀ is a possible solution, then Φ₀ + ψΔ is a solution for any ψ ∈ R, with Δ: (28) Δ = ( T c , x 1 T c , x 2 a b ( 0 0 T c , x 3 ) T ⋯ ( 0 0 T c , x 3 ) T )

In fact, if Δ is normalized, it matches up with the eigen-vector associated to the zero eigenvalue of matrix A^T A.

Using the constraint that a² + b² = 1, and the singular value decomposition of matrix A, an exact solution of system (20) is obtained.

Once Φ is available, the solution X 0 a is obtained by inverting the parametrization: (29) α 0 = tan − 1 ( a , b ) M i = R 0 − 1 M 0 i X 0 a = ( x 0 1 x 0 2 α 0 ( M 1 ) T ⋯ ( M N ) T ) T

This method, although exact, is prone to error due to odometry noise and does not benefit from the Maximum Likelihood metric. However it is valid as a method to give an initial value for X 0 a before using the iterative approach.

The kind of trajectory performed by the robot during initialization has direct influence in the solution of X 0 a. There are three kinds of movements that yields degenerate solutions:

Straight motion: there is no information about the center of rotation of the robot and thus the pose has multiple solutions.

In practical cases it has been proved to be effective enough to combine straight trajectories with circular motion to avoid degeneracies in the solution of X 0 a.

Once the minimum of (16) is reached we suppose that the resulting value of X 0 a is the mean of the distribution X 0 a. This section describes how to also obtain the covariance matrix.

The covariance matrix ∑ 0 a of the optimized parameters is easily obtained by using a local approximation of the term Y_L − Ŷ_L in the vicinity of the minimum X ^ 0 a using the following expression: (34) ∑ 0 a = ( J T ∑ L − 1 J ) − 1where J is the Jacobian matrix of Ŷ with respect to parameters X 0 a. The Jacobian is available from the optimization method, in which is used to compute the iteration steps.

The estimation step uses the motion models available to infer the next pose of the robot which implies an increment in uncertainty. Starting from the last pose distribution p ( X k − 1 a | Y 1 , ⋯ , Y k − 1 ) = N ( X ^ k − 1 a , ∑ k − 1 a ), the motion model g^a and the noise included in odometry values, the following update is obtained: (35) X ^ k | k − 1 a = g a ( X ^ k a , U k ) ∑ k | k − 1 a = J x T ∑ k − 1 a J x + J U T ∑ W J U ,where X ^ k | k − 1 a and ∑ k | k − 1 a are the mean and covariance matrix of distribution: p ( X k a | Y 1 , ⋯ , Y k − 1 )

The matrices J_X and J_U are the first derivatives of the function g^a with respect to X k − 1 a and U_k respectively. Usually J_X in odometry systems is the identity, therefore, at this step, the covariance matrix ∑ k | k − 1 a results to be bigger in terms of eigenvalues, which means uncertainty.

The correction step removes the added uncertainty in the estimation by using image measurements. It passes from the distribution p ( X k a | Y 1 , ⋯ , Y k − 1 ) to the target distribution p ( X k a | Y 1 , ⋯ , Y k ), which includes the last measurement.

Using the estimation shown in (35), and knowing the correspondence between measurements with the camera and structure point of the state vector, the estimated measurement is given: (36) Y k | k − 1 = h a ( X k a ) (37) ∑ Y k | k − 1 = J h T ∑ k | k − 1 a J h + ∑ V (38) ∑ X a Y = ∑ k | k − 1 a J hwhere J_h is the Jacobian matrix of the function h^a with respect to X k a and ∑_V is block diagonal matrix with ∑_v on each block. Function h^a performs the projection in the image plane of the camera of all visible points that form up the measurement vector Y_k. The correction step itself is a linear correction of X k | k − 1 a and ∑ k | k − 1 a by means of the Kalman gain K_G: (39) K G = ∑ X a Y ∑ Y k | k − 1 − 1 (40) X k a = X k | k − 1 a + K G ( Y k − Y k | k − 1 ) (41) ∑ k a = ∑ k | k − 1 a − K G ∑ X a Y T

As it is stated in (41) the resulting ∑ k a is reduced compared to ∑ k | k − 1 a which means that after the correction step, the uncertainty is “smaller”.

The robust layer has the objective of removing bad measurements from Y_k to avoid inconsistent updates of X k a in the correction step. In this paper we propose to include the Random Sample Consensus (RANSAC) algorithm [22] between the estimation and correction step of the filter. The general idea is to found among the measured data Y_k a set which agrees in the pose X_k, using the 3D model obtained in X k − 1 a.

The interest of applying RANSAC in a sequential update approach resides on several reasons: firstly it allows to efficiently discard outliers from Y_k preventing algorithm’s degeneracy, which happens even if the motion model is accurate. Secondly, compared to online robust approaches, where a robust cost function is optimized, RANSAC allows not to break the Kalman filter approach, as it only cleans the measurement vector of outliers. Furthermore we have observed experimentally that the RANSAC algorithm can be very fast between iterations, as only a few outliers are inside the data. (We use the RANSAC implementation described in [17], which implements a dynamical computation of the outlier probability).

The RANSAC method proposed in the commented framework obtains the consensus pose X_k from the set of measurements Y_k and the 3D model available in X k − 1 a using the algorithm presented in [18]. For a robot moving in a plane, as it is the case with the mobile robot considered in this paper, it is enough to use a minimum of 2 correspondences between the model and the measurement which makes the RANSAC very fast for removing outliers.

Contrary to the initialization case, in this step we have an accurate prediction of the tracked points in the image plane at each time instant, namely vector Y_k|k−1. Using such prediction we can easily match the 3D points in the state vector with measurements taken in a measurement set 𝒡_k, using the SIFT detector applied to current image I_k. The matching is done in terms of distance in the image plane. Let y k | k − 1 i a feature inside vector Y_k|k−1 and y k j a candidate obtained using SIFT method. We conclude that they are matched if | y k j − y k | k − 1 i | M 2 < τ max, where ‖_M states for the Mahalanobis distance using the covariance ∑_yk|k−1 computed from the matrix ∑_Yk|k−1. The Mahalanobis distance allows to take into account the uncertainty predicted for y k | k − 1 i and also helps to select a threshold τ_max with a probabilistic criterion.

In this experiment the robot structure is simulated with a set of N = 10 points distributed randomly inside a cylinder with radius R = 0.5m and height h = 1m. The odometry system is supposed to have an uncertainty σ Vl 2 = 10 and σ Ω 2 = 1 in linear and angular speed respectively. The initialization trajectory is shown in Figure 8. The image measurements are considered polluted with a Gaussian process of σ v 2 = 10, independently of each measurement and image coordinate.

To compare the performance of the initialization method proposed in Section 3., the following error magnitudes are described: (43) ɛ M = ∑ i = 1 N ‖ M i − M gth i ‖ 2 ∑ i = 1 N ‖ M gth i ‖ 2       ɛ T = ‖ T 0 − T 0 , gth ‖       ɛ α =   ‖ α 0 − α 0 , gth ‖where T_0,gth, α_0,gth and M gth i correspond to the ground truth values of the initialization vector X 0 a. The following two experiments are proposed:

Initialization errors in function of the odometry error. In this experiment the value of σ Vl 2 and σ Ω 2 are multiplied by the following multiplicative factor: (44) ρ ∈ ( 0.01 0.1 0.5 1 5 10 )

The different errors of Equation 43 can be viewed in Figure 9a–c in terms of ρ. As it can be observed in the results, the M.C. (Complete correlated matrix ∑_L) method outperforms the rest of approximations of ∑_L, specially the full diagonal method M.I., which means that the statistical modelling proposed in this paper is effective.

Both experiments clearly manifest that the complete matrix ∑_L, with all its cross-correlated terms (M.C.), outperforms the rest of proposals, especially when it is compared to the case where ∑_L is the identity matrix (M.I.), which means no statistical modeling.

The initialization experiment proposed in this paper consists of a robot performing a short trajectory from which its 3D model and initial position is obtained using the results of Section 3.. We present a comparison of the initialization results using three different trajectories. Each one of the trajectories is displayed in Figure 11 as a colored interval of the whole trajectory performed by the robot in the experiment.

The results of the initialization method on each of the 3 trajectories selected is shown in Figure 12. Depending on the trajectory used, the features viewed by the camera vary and thus the corresponding initialized 3D model. As it can be seen in Figure 12, the 3D model is accurate and its projection matches with points in the robot’s structure in all cases. It must be remarked that on each case, the position obtained as a result of the initialization (i.e., X₀) corresponds to the first position of each interval.

The sequential algorithm is tested using the whole trajectory shown in Figure 11. The initial pose and 3D model are the result of the initialization results shown in Figure 12e,f. The estimated position of the robot, compared to a “ground truth” measurement, is presented in Figure 13a,b.

The ground truth is obtained by means of a manually measured 3D model of the robot. The model is composed of 3D points, that are easily recognized by a human observer. By manually clicking points of the 3D model in the image plane, and by using the method proposed in [18], the reference pose of the robot is obtained in some of the frames of the experiment.

Another experiment is proposed to test the online algorithm with occlusions and a larger path followed by the robot. In Figure 14a it is shown the geometric placement of the camera and in Figure 14b it is shown the geometric model obtained during initialization.

The resulting localization error is shown in Figure 15b. In Figure 16 it is shown several frames, indexed by the time sample number k, presenting hard occlusions between the camera and the robot without losing the tracking. The RANSAC method used in the Kalman loop avoid erroneous matches in the occluded parts of the object. Besides, in those frames with completely occluded features, only the estimation step of the Kalman filter is done, which is accurate enough for short periods of time.

In both sequential experiments the 3D model is composed of 8 to 10 points and no extra geometrical information is introduced in the state vector. As a future line of study a simultaneous reconstruction and localization approach can be adopted to introduce online extra 3D points in the state vector as the robot is tracked. A very similar approach is done in the Simultaneous Localization and Mapping (SLAM) problem, and some of their solutions [5] are perfectly applicable to the problem assessed in this paper.