Jorge Othón Esparza-Jiménez
email: jorge.othon.esparza.jimenez@laas.fr

Michel Devy
email: michel.devy@laas.fr

J L Gordillo
email: jlgordillo@itesm.mx

EKF-based SLAM fusing heterogeneous landmarks

Abstract-Visual SLAM (Simultaneous Localization and Mapping from Vision) concerns both the spatial and temporal fusion of sensory data in a map when moving a camera in an unknown environment. This paper concerns the construction of landmarks-based stochastic map, using Extended Kalman Filtering in order to fuse new observations in the map, when considering heterogeneous landmarks. It is evaluated how this combination allows to improve the accuracy both on the map and on the camera localization, depending on the parametrization selected for points and straight lines. It is analyzed using a simulated environment, so knowing perfectly the ground truth, what are the better landmark representations. Experiments on image sequences acquired from a camera mounted on a mobile robot, were already presented: it is detailed here a new front end where segment matching has been improved.

I. INTRODUCTION

SLAM was introduced twenty-five years ago as a function required for a robot, to build a map from observations acquired by embedded sensors, while exploring an unknown environment, . It has been proposed many formal approaches to deal with the fusion of observations in the map (estimation, optimization, interval analysis. . . ), and many representations for the environment (landmarks, grids, raw data. . . ) [START_REF] Durrant-Whyte | Simultaneous Localization and Mapping (SLAM): Part I & II[END_REF]. While it is built, the map must allow the robot to estimate its pose; so many SLAM approaches are only devoted to build a landmarks-based map, i.e. a sparse model made with distinctive and characteristic entities located in the 3D space, which correspond without ambiguity, to some observed features.

Visual SLAM has been studied since 12 years [START_REF] Davison | Real-time simultaneous localisation and mapping with a single camera[END_REF], using at first an estimation framework. It could find applications not only in robotics, but also for the introduction of new services using smart devices equipped with a camera (smart phones, Kinect. . . ). Landmarks are typically 3D points or 3D lines, that are observed by points or segments features in images; one observation does not allow to initialize such a landmark, while a 3D point is observed by an optical ray (i.e. a 3D straight line), and a 3D line is observed by an interpretation plane.

So in order to initialize such landmarks with their minimal euclidean parameters, it is necessary to wait for other observations. It is the reason why the first strategy proposed in [START_REF] Davison | Real-time simultaneous localisation and mapping with a single camera[END_REF] [3] applied a delayed initialization; a landmark was added in the map only when it was known in the euclidean space. This approach is unable to use landmarks that are very far from the robot. So several parametrizations have been proposed for points [START_REF] Montiel | Unified inverse depth parametrization for monocular slam[END_REF] [START_REF] Sola | Undelayed initialization in bearing only slam[END_REF] or lines [START_REF] Sola | Undelayed initialization of line segments in monocular slam[END_REF] landmarks, for their undelayed initialization, i.e. they are added in the map as soon as they are observed. Solá and al [START_REF] Sola | Impact of landmark parametrization on monocular ekf-slam with points and lines[END_REF] have analyzed the pros and cons of several representations for 3D points and 3D lines, before they can be triangulated with a sufficient accuracy.

Here this analysis is made more complete using an heterogeneous map, where points and lines are both initialized as soon as they are detected from features extracted in images acquired by a camera moved in the environment. It is now known [START_REF] Strasdat | Real-time monocular slam: Why filter?[END_REF] that optimizationbased methods like PTAM [START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF] or methods based on the g2o library [START_REF] Kuemmerle | g2o: A general framework for graph optimization[END_REF] allow to avoid the possible divergence of methords based on estimation, due to linearization of the observation model. Nevertheless here the fusion is performed from an EKF-based SLAM method, as a very light approach that can be integrated on dedicated architecture using co-design methodology, to be used on small aerial vehicles.

In the next section it is recalled the different parametrization proposed for points and lines landmarks. Then the sections III and IV summarize the way landmarks are initialized as soon as they are observed, and then updated from next observations. Finally, It is analyzed in section V, what are the best landmarks representations when a heterogeneous map is built. Also, in the section VI, it is shown some results obtained on image sequences acquired from a mobile robot moving on a road close to buildings.

II. LANDMARK PARAMETERIZATION

In [START_REF] Sola | Impact of landmark parametrization on monocular ekf-slam with points and lines[END_REF], Solà et al. introduce an undelayed landmark initialization (ULI) for different points and lines parameterizations. It consists on substituting the unmeasured degrees of freedon by a Gaussian prior that handles infinite uncertainty but that is still manageable by EKF.

For point and line landmark , uncertainty has distinct implications. For points, there is uncertainty in depth and it covers all the visual ray until infinity. Infinite straight lines handles uncertainty in two degrees of freedom, which correspond to a depth that should be covered up to infinity, and all possible orientations.

A. 3D point parameterizations

This section explains some point parameterizations. The aspects included in each description refer to the parameterization itself, camera projection, coordinate transformation, and back-projection.

1) Euclidean point:

The parameters of an Euclidean point consist on its Cartesian coordinates.

L EP = p = [x y z] T ∈ R 3
The projection to camera frame is given by the following equation:

u = KR T (p -T) ∈ P 2 (1)
where,

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ . (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point.

ρ = m d
This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m /d min ].

The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera frame, the projected image point is u = Km C , and ρ C is not measurable. Back-projection is then:

m C = K -1 u.
The complete homogeneous point parameterization is given in the following equations:

L HP = p = m ρ = H K -1 u ρ C = RK -1 u + Tρ C ρ C (5)
, where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

Homogeneous point parameterization is shown in figure 1. For different landmark types such as points and lines, uncertainty has distict implications. For points, there is uncertainty in distance and it covers all the visual ray until infinity. Infinite straight lines handles uncertainty in two degrees of freedom, which correspond to a distance that should be covered up to infinity, and all possible orientations. In the next sections, point and line parameterizations used for ULI are covered, for then deepen in the initialization process itself.

A. 3D point parameterizations

This section explains some point parameterizations. The aspects included in each description refer to the parameterization itself, camera projection, coordinate transformation, and back-projection.

L EP = p = [x y z] T ∈ R 3
The projection to camera frame is given by the following equation:

u = KR T (p -T) ∈ P 2
(1)

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ . (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera For different landmark types such as points and lines, uncertainty has distict implications. For points, there is uncertainty in distance and it covers all the visual ray until infinity. Infinite straight lines handles uncertainty in two degrees of freedom, which correspond to a distance that should be covered up to infinity, and all possible orientations. In the next sections, point and line parameterizations used for ULI are covered, for then deepen in the initialization process itself.

A. 3D point parameterizations

This section explains some point parameterizations. The aspects included in each description refer to the parameterization itself, camera projection, coordinate transformation, and back-projection. 2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ . (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression: For different landmark types such as points and lines, uncertainty has distict implications. For points, there is uncertainty in distance and it covers all the visual ray until infinity. Infinite straight lines handles uncertainty in two degrees of freedom, which correspond to a distance that should be covered up to infinity, and all possible orientations. In the next sections, point and line parameterizations used for ULI are covered, for then deepen in the initialization process itself.

EKF-SLAM with heterogeneous landmarks

A. 3D point parameterizations

This section explains some point parameterizations. The aspects included in each description refer to the parameterization itself, camera projection, coordinate transformation, and back-projection.

L EP = p = [x y z] T ∈ R 3
The projection to camera frame is given by the following equation:

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ , d = m/ρ. (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
EKF-SLAM with heterogeneous landmarks For different landmark types such as points and lines, uncertainty has distict implications. For points, there is uncertainty in distance and it covers all the visual ray until infinity. Infinite straight lines handles uncertainty in two degrees of freedom, which correspond to a distance that should be covered up to infinity, and all possible orientations. In the next sections, point and line parameterizations used for ULI are covered, for then deepen in the initialization process itself.

A. 3D point parameterizations

This section explains some point parameterizations. The aspects included in each description refer to the parameterization itself, camera projection, coordinate transformation, and back-projection.

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ .O (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed. 3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . ( 6
)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

Anchored homogeneous point parameterization is shown in figure 2.

P = p = [x y z] T ∈ R 3
n to camera frame is given by the n:

= KR T (p -T) ∈ P 2 (1)
K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ . (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera frame, the projected image point is u = Km C , and ρ C is not measurable. Back-projection is then: th heterogeneous landmarks

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ . (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera frame, the projected image point is u = Km C , and ρ C is not measurable. Back-projection is then: with heterogeneous landmarks (1)

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ , d = m/ρ. (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera frame, the projected image point is u = Km C , and ρ C is not measurable. Back-projection is then:

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ .O (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera frame, the projected image point is u = Km C , and ρ C is not measurable. Back-projection is then:

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates:

L P L n v = [n x n y n z v v y v z ] T ∈ P 5 ⊂ R 6 ,
where n = a × b, n = abba, n, v ∈ R 3 , and the following Plücker constraint:

n T v = 0.
Geometrically speaking, n is the vector normal to the plane π containing the line and the origin, and v is the director vector from a to b. The Euclidean orthogonal distance from the line to the origin is given by n / v . Thus, v is the inverse-depth, analogous to ρ of homogeneous points. Plücker line geometrical representation is shown in figure 3.

EKF-SLAM with heterogeneous landmarks Michel Devy

Jorge Othón Esparza-Jiménez For different landmark types such as points and lines, uncertainty has distict implications. For points, there is uncertainty in distance and it covers all the visual ray until infinity. Infinite straight lines handles uncertainty in two degrees of freedom, which correspond to a distance that should be covered up to infinity, and all possible orientations. In the next sections, point and line parameterizations used for ULI are covered, for then deepen in the initialization process itself.

A. 3D point parameterizations

This section explains some point parameterizations. The aspects included in each description refer to the parameterization itself, camera projection, coordinate transformation, and back-projection.

L EP = p = [x y z] T ∈ R 3
The projection to camera frame is given by the following equation:

u = KR T (p -T) ∈ P 2 (1)
where,

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ .O (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera frame, the projected image point is u = Km C , and ρ C is not measurable. Back-projection is then:

m C = K -1 u.
The complete homogeneous point parameterization is given in the following equations:

L HP = p = m ρ = H K -1 u ρ C = RK -1 u + Tρ C ρ C (5)
, where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . (6)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
where n = a × b, n = abba, n, v ∈ R 3 , and having the following Plücker constraint:

n T v = 0.
Geometrically speaking, n is the vector normal to the plane π containing the line and the origin, and v is the director vector from a to b. The Euclidean orthogonal distance from the line to the origin is given by n/v. Thus, v is the inverse-depth, analogous to ρ of homogeneous points. Plücker line geometrical representation is shown in figure 2. Plücker coordinates transformation from camera frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions:

2 m C = K -1 u.
The complete homogeneous point parameterization is given in the following equations:

L HP = p = m ρ = H K -1 u ρ C = RK -1 u + Tρ C ρ C (5)
, where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . (6)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
where n = a × b, n = abba, n, v ∈ R 3 , and having the following Plücker constraint:

n T v = 0.
Geometrically speaking, n is the vector normal to the plane π containing the line and the origin, and v is the director vector from a to b. The Euclidean orthogonal distance from the line to the origin is given by n/v. Thus, v is the inverse-depth, analogous to ρ of homogeneous points. Plücker line geometrical representation is shown in figure 2. Plücker coordinates transformation from camera frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions: 2

RK -1 u + Tρ C ρ C ,
where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . ( 6
)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
to ρ of homogeneous points. Plücker line geometrical representation is shown in figure 2. Plücker coordinates transformation from camera frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) d = n v ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions: 2 inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . (6)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates: Plücker coordinates transformation from camera frame is performed as shown next:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) π ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions:

2 m C = K -1 u.
The complete homogeneous point parameterization is given in the following equations:

L HP = p = m ρ = H K -1 u ρ C = RK -1 u + Tρ C ρ C (5)
, where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . (6)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
where n = a × b, n = abba, n, v ∈ R 3 , and having the following Plücker constraint:

n T v = 0.
Geometrically speaking, n is the vector normal to the plane π containing the line and the origin, and v is the director vector from a to b. The Euclidean orthogonal distance from the line to the origin is given by n/v. Thus, v is the inverse-depth, analogous to ρ of homogeneous points. Plücker line geometrical representation is shown in figure 2. Plücker coordinates transformation from camera frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) π ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions:

2 m C = K -1 u.
The complete homogeneous point parameterization is given in the following equations:

L HP = p = m ρ = H K -1 u ρ C = RK -1 u + Tρ C ρ C (5)
, where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . ( 6
)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
where n = a × b, n = abba, n, v ∈ R 3 , and having the following Plücker constraint:

n T v = 0.
Geometrically speaking, n is the vector normal to the plane π containing the line and the origin, and v is the director vector from a to b. The Euclidean orthogonal distance from the line to the origin is given by n/v. Thus, v is the inverse-depth, analogous to ρ of homogeneous points. Plücker line geometrical representation is shown in figure 2. Plücker coordinates transformation from camera frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) π ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions:

2 ρ
The convertion from anchred homogeneous po Euclidean coordinates can be achieved by the foll equation:

p = p 0 + m ρ .
The projection and frame transformation proc given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 .
The complete anchores homogeneous point p eterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   ,
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations ar ered. The description of projection to image fram linear transformation and back-projection are inc T can be represen homogeneous 6-vector, known as Plücker coordi Plücker coordinates transformation from camera frame is performed as shown next:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂
L P L = H L C L = [T] × R 0 • n v C
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) C ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 u v 0 u α v
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions: Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

n C = K -1 • l v C = β 1 • e 1 + β 2 • e 2 β 1 , β 2 ∈ R
v C = E • β,
where v C ∈ π C for any value of β.

Plücker line back projection is shown in figure 4.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C ( 14
)
e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 ( 12
)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14)
e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . (18)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m × m is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v. 3

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 ( 12
)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

L HP = p = m ρ = H K -1 u ρ C = RK -1 u + Tρ C ρ C (5)
where ρ C must be given as prior and represents verse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to imove linearity, an anchor is added as a reference to e optical center at initialization time of the landmark. us, the landmark is a 6-vector that includes the anchor coordinates, the Cartesian coordinates of the point ith respect to the anchor, and an inverse-depth scalar.

AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to clidean coordinates can be achieved by the following uation:

p = p 0 + m ρ . ( 6
)
The projection and frame transformation process is ven in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point paramerization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , ( 8
)
where ρ C must be given as prior.

3D line parameterizations

In this section, some line parameterizations are coved. The description of projection to image frame, biear transformation and back-projection are included. T can be represented as mogeneous 6-vector, known as Plücker coordinates:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
Geometrically speaking, n is the vector normal to the plane π containing the line and the origin, and v is the director vector from a to b. The Euclidean orthogonal distance from the line to the origin is given by n/v. Thus, v is the inverse-depth, analogous to ρ of homogeneous points. Plücker line geometrical representation is shown in figure 2. Plücker coordinates transformation from camera frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) π ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions: 2 includes the anchor inates of the point nverse-depth scalar.

m y m z ρ] T ∈ R 7 .
mogeneous point to ed by the following [START_REF] Sola | Undelayed initialization of line segments in monocular slam[END_REF] ormation process is

ρ) ∈ P 2 . ( 7
)
neous point param-

T -1 u ρ C   , (8)
r.

terizations are covto image frame, biection are included.

fined by two points be represented as lücker coordinates: Plücker coordinates transformation from camera frame is performed as shown next:

v z ] T ∈ P 5 ⊂ R 6 ,
L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) π C ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions: Plücker coordinates transformation from camera frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) I ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions: sentation is shown in figure 2.

L P L = H • L C P L = R [T] × R 0 R • n C v C .
he whole transformation and projection process for ker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) C ( 9
)
here K is the instrinsic projection Plücker matrix ed as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v .
hen Plücker coordinates are expressed in camera e, projection is only obtained by

l = K • n C (10)
ine's range and orientation expressed in v C are not urable.

or Plücker line back projection, vectors n C and v C omputed according to these expressions: The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (11)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L AHP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) (12)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 . ( 13
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v. For different landmark types such as points and lines, uncertainty has distict implications. For points, there is uncertainty in distance and it covers all the visual ray until infinity. Infinite straight lines handles uncertainty in two degrees of freedom, which correspond to a distance that should be covered up to infinity, and all possible orientations. In the next sections, point and line parameterizations used for ULI are covered, for then deepen in the initialization process itself.

A. 3D point parameterizations

This section explains some point parameterizations. The aspects included in each description refer to the parameterization itself, camera projection, coordinate transformation, and back-projection.

L EP = p = [x y z] T ∈ R 3
The projection to camera frame is given by the following equation:

u = KR T (p -T) ∈ P 2 (1)
where,

K = α u 0 u 0 0 α v v 0 0 0 1
R and T are the rotation matrix and translation vector that define the camera C. Underlined vectors like u represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points are conformed by a 4-vector, which is composed by the 3D vector m and scalar ρ.

L HP = p = m ρ = [m x m y m z ρ] T ∈ P 3 ⊂ R 4
In order to convert from homogeneous to Euclidean coordinates, the following equation is applied:

p = m ρ .O (2)
In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence to the inverse of the distance d defined from the optical center to the point. The frame transformation of an homogeneous point is performed according to the next equation:

p = Hp C = R T 0 1 p C , (3)
where super-index C indicates the frame to which the point is refered, and matrix H sepecifies the frame to which the point is transformed.

The projection of a point into the image frame is performed with the following expression:

u = KR T (m -Tρ) ∈ P 2 . ( 4
)
Expressing an homogeneous point in the camera frame, the projected image point is u = Km C , and ρ C is not measurable. Back-projection is then:

m C = K -1 u.
The complete homogeneous point parameterization is given in the following equations:

L HP = p = m ρ = H K -1 u ρ C = RK -1 u + Tρ C ρ C (5)
, where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

L AHP = p 0 m ρ = [x 0 y 0 z 0 m x m y m z ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p 0 + m ρ . (6)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p 0 ) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

L AHP = p 0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates:

L P L = n v = [n x n y n z v x v y v z ] T ∈ P 5 ⊂ R 6 ,
where n = a × b, n = abba, n, v having the following Plücker constraint: n T Geometrically speaking, n is the vecto the plane π containing the line and the v is the director vector from a to b. Th orthogonal distance from the line to the ori by n/v. Thus, v is the inverse-depth to ρ of homogeneous points. Plücker line representation is shown in figure 2. Plücker coordinates transformation fr frame is performed as shown next:

L P L = H • L C P L = R [T] × R 0 R •
The whole transformation and projection Plücker coordinates in terms of R , T, n, a

l = K • R T • (n -T × v)
where K is the instrinsic projection Plü defined as:

K = α v 0 0 0 α u 0 -α v u 0 α u v 0 α u α v
When Plücker coordinates are expresse frame, projection is only obtained by

l = K • n C
Line's range and orientation expressed in measurable.

For Plücker line back projection, vectors are computed according to these expression 2 , where ρ C must be given as prior and represents inverse-distance from the origin of coordinates.

3) Anchored homogeneous point: In order to improve linearity, an anchor is added as a reference to the optical center at initialization time of the landmark. Thus, the landmark is a 6-vector that includes the anchor 3D coordinates, the Cartesian coordinates of the point with respect to the anchor, and an inverse-depth scalar.

LAHP = p0 m ρ = [x0 y0 z0 mx my mz ρ] T ∈ R 7 .
The convertion from anchred homogeneous point to Euclidean coordinates can be achieved by the following equation:

p = p0 + m ρ . (6)
The projection and frame transformation process is given in the next expression:

u = KR T (m -(T -p0) ρ) ∈ P 2 . ( 7
)
The complete anchores homogeneous point parameterization is the following:

LAHP = p0 m ρ =   T RK -1 u ρ C   , (8)
where ρ C must be given as prior.

B. 3D line parameterizations

In this section, some line parameterizations are covered. The description of projection to image frame, bilinear transformation and back-projection are included. T can be represented as homogeneous 6-vector, known as Plücker coordinates: Plücker coordinates transformation from camera frame is performed as shown next:

LPL = n v = [nx ny nz vx vy vz] T ∈ P 5 ⊂ R 6 ,
LPL = H • L C P L = R [T] × R 0 R • n C v C .
The whole transformation and projection process for Plücker coordinates in terms of R , T, n, and v is:

l = K • R T • (n -T × v) π ( 9
)
where K is the instrinsic projection Plücker matrix defined as:

K = αv 0 0 0 αu 0 -αvu0 αuv0 αuαv .
When Plücker coordinates are expressed in camera frame, projection is only obtained by

l = K • n C (10)
Line's range and orientation expressed in v C are not measurable.

For Plücker line back projection, vectors n C and v C are computed according to these expressions:

2 n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14)
e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) m 1 /ρ 1 (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) m 2 /ρ 2 (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) m 2 (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) m 1 (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 (12)
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) p 1 (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β 1 • e 1 + β 2 • e 2 3
where β 1 , β 2 ∈ R and e 1 , e 2 , n C are mutually orthogonal.

Defining β = (β 1 , β 2 ) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e 1 be parallel to the image plane, e 1 and e 2 are obtained with the next expressions:

e 1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e 2 = n C × e 1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

L P L = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

L HP L =      p 0 m 1 ρ 1 m 2 ρ 2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (m i -(T -p 0 ) ρ i ) p 2 (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m 1 × m 2 ) -(T -p 0 ) × (ρ 1 m 2 -ρ 2 m 1 )) . ( 18
)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m 1 × m 2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ 1 m 2ρ 2 m 1 ) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v.

n C = K -1 • l (11) v C = β1 • e1 + β2 • e2 3
where β1, β2 ∈ R and e1, e2, n C are mutually orthogonal.

Defining β = (β1, β2) ∈ R 2 , vector v C can be also expressed as:

v C = E • β, (13)
where v C ∈ π C for any value of β.

In order for β to be exactly inverse-distance and e1 be parallel to the image plane, e1 and e2 are obtained with the next expressions:

e1 = n C 2 -n C 1 0 T n C 1 2 + n C 2 2 • n C (14) e2 = n C × e1 n C (15)
Plücker line back projection is shown in figure 3. The complete Plücker line parameterization is the following:

LPL = H n C v C = H K -1 l Eβ = RK -1 l + T × REβ REβ , (16)
where β must be provided as a prior.

2) Anchored Homogeneous-points line: Another way of representing a line is by the endpoints that define it. Departing from the anchored homogeneous point parameterization, an homogeneous-point line is an 11-vector defined as follows:

LAHP L =      p0 m1 ρ1 m2 ρ2      ∈ R 11
For each point, the transformation and projection of a pinhole camera is , as previously stated,

u i = KR T (mi -(T -p0) ρi) p2 (17)
An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u 1 × u 2 and thus,

l = KR T ((m1 × m2) -(T -p0) × (ρ1m2 -ρ2m1)) . (18)
Comparing this result to what was obtained for Plücker coordinates, it can be seen that the product m1 × m2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Also, the term (ρ1m2 -ρ2m1) is a vector joining the two support points of the line, therefore related to Plücker sub-vector v. In the case of lines, they can be expressed as bounded segments by means of their endpoints in a 4vector, also with a Gaussian probability distribution.

s = u 1 u 2 ∼ N {s, S} = N ū1 ū2 , U 0 0 U
The probability function for infinite lines like Plücker, pdf N l, L , uses the homogeneous line representation previously shown, and the Gaussian distribution defined by : l = ū1 × ū2 , and L

= [ū 1 ] × U [ū 1 ] T × + [ū 2 ] × U [ū 2 ] T ×
The uncertainty in 3D points and lines coming from projection is and modeled in inverse-distance priors ρ C and β C through Gaussian variables. The origin of each of these priors must be inside the 2σ of the their probability density functions.

For points and point supported lines, the minimum distance must match the upper 2σ bound, thus: This penalizes lines at the back of the camera.

ρ -nσ ρ = 0, 0 ≤ n < 2 ρ + 2σ ρ = 1/

A. Undelayed landmark initialization algorithm

The ULI algorithm, as presented in [START_REF] Sola | Impact of landmark parametrization on monocular ekf-slam with points and lines[END_REF] is composed by the followings steps: The landmark update process starts by projecting all landmarks to the image plane, and selecting those with higher uncertainty to be corrected. In the case of points, the observation function h() applies an homogeneous to Euclidean transformation h2e() to the transformation + projection proceses previously presented. For lines, observation function computes the orthogonal distances from the detected endpoints u i to a line l.

z = l T • u 1 / l 2 1 + l 2 2 l T • u 2 / l 2 1 + l 2 2 ∈ R 2
Being the EKF innovation, the difference between the actual measurement and the expectation y = zh (x), z is the orthogonal distance from the endpoints to the line defined by them. Thus

y = 0 -h (x)
A landmark is found consistent if the squared Mahalanobis distance M D2 of innovation is smaller than a threshold M D2th

M D2 = y T • Y -1 • y < M D2th
Being that the case, landmark is updated Kalman gain:

K = P • H • Y -1
State update: x ← x + K • y Covariance update: P ← P -K • H • P

V. EXPERIMENTAL RESULTS

This section presents the results of the simulation experiments performed for comparing the different point and lines parameterizations with heterogeneous approches that combine points and lines in the same map.

The experiments were performed in MATLAB , departing from the EKF-SLAM toolbox [START_REF] Sola | An EKF-SLAM toolbox for MATLAB[END_REF] and adding the heterogeneous functionality.

The following parameterizations were evaluated:

• Anchored Homogenenous Point (AHP)

• Plücker Line (PL)

• Anchored Homogenenous Point Line (AHPL)

• AHP + PL

• AHP + AHPL

The environment consisted on a house conformed by 23 lines and an array of 16 points distributed equally among the walls of the house. The environment for the simulation is shown in figure 6.

The robot performs a circular trajectory of 5 m of diameter, with a pose step of 8 cm and 0.09 • . the linear noise is 0.5 cm and the angular noise 0.05 • In addition to the heterogeneous landmark handling of the toolbox, two more considerations were integrated to evaluate the performance of the parameterizations in a more realistic way. The first one was the transparency of the objects in the scene. Normally, the objects in the simulaton enviroment of the toolbox are transparent, allowing to have an almost complete view of the landmarks during all the time steps. An aspect graph was implemented in order to see only the visible surfaces of the house from each camera position. Also, by default the algorithm is aware of loops, so it automatically performs feature matching, which allows to have a more accurate position estimation. It was modified in order to have the capability of initializing the landmarks as new ones on each turn and evaluate the performance in that case too. Thus, four different setup conditions were considered:

• Transparent objects with loop acquaintance.

• Transparent objects without loop acquaintance.

• Opaque objects with loop acquaintance.

• Opaque objects without loop acquaintance.

Figure 7 shows the sensor view considering transparent objects, while figure 8 shows the case with opaque objects. Figure 9 gives an example of the environment displayed after a complete turn with Plücker line + inverse depth point parameterization. It displays in green the line landmarks estimated, and in blue the point landmarks. Real, predicted, and estimated robot trajectories are displayed in blue, red, and green respectively.

A trajectory of 5 turns was performed for each parameterizaton and each condition mentioned earlier. Among the parameterizations, the highest error correspond to Plücker line. The best performance correspond to the anchored parameterizations, both for points and lines. It can be seen the improvement effect in Plücker line parameterization by the addition of points. Even for the anchored parameterizations, already having a relatively well performance while working independently, the heterogeneity brings benefits, in such a way that the combination of both AHP and AHPL is

VI. REAL IMAGES

The evaluation on real images is on the way; some results have been already presented, especially in [START_REF] Vidal-Calleja | Large scale multiple robot visual mapping with heterogeneous landmarks in semi-structured terrain[END_REF]. Here the contribution concerns the integration of the LSD segment detector presented in [START_REF] Grompone Von Gioi | LSD: a Line Segment Detector[END_REF] and a moving edge tracker based on [START_REF] Bouthemy | A maximum likelihood framework for determining moving edges[END_REF] [START_REF] Comport | Real-time markerless tracking for augmented reality: the virtual visual servoing framework[END_REF]. Figure 14 shows a set of frames of a sequence that have been processed for the tracking of automatically detected linear segments, and the detecton of points.

VII. CONCLUSION

This paper intends to prove the benefits of considering heterogeneous landmarks when building a map from an EKF-based visual SLAM method. Using only monocular vision, only partial observations of landmarks are provided by features extracted from images; here it is used undelayed initialization of landmarkslike it was proposed initially by Solaánd al [5] [6] for points and segments. It has been shown using simulated data, how the choice of the landmarks representation has an impact on the map accuracy. Finally the best ones considering the construction of map with heterogeneous landmarks, are Anchored Homogeneous Points and Anchrored Homogeneous-PointsLines.

Experiments with real images are on the way, and some preliminary results have been presented on line tracking using different sequences acquired in urban environment. In the future works, constraints will be exploited in the map, typically when points and segments are extracted from the same facades.

France. The authors would like to thank CONACYT for the funding of this stay; this work contributes to the french FUI project AIR-COBOT.

  ], Solà et al. introduce an undelayed landmark initialization (ULI) for different points and lines parameterizations. It consists on substituting the unmeasured degree of freedon by a Gaussian prior that handles infinite uncertainty but that is still manageable my the EKF.

Figure 1 :

 1 Figure 1: Point parameterizations. Image extracted from [?] 1) Euclidean point: The parameters of an Euclidean point consist on its Cartesian coordinates.

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

Figure 1 :R

 1 Figure 1: Point parameterizations. Image extracted from [?] 1) Euclidean point: The parameters of an Euclidean point consist on its Cartesian coordinates.T

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

Figure 1 :

 1 Figure 1: Point parameterizations. Image extracted from [?] 1) Euclidean point: The parameters of an Euclidean point consist on its Cartesian coordinates.

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

Figure 1 :

 1 Figure 1: Point parameterizations. Image extracted from [?]

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

Figure 1 :

 1 Figure 1: Homogeneous point parameterization.

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

Figure 2 :

 2 Figure 2: Anchored homogeneous point parameterization.

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

  ], Solà et al. introduce an undelayed landmark initialization (ULI) for different points and lines parameterizations. It consists on substituting the unmeasured degree of freedon by a Gaussian prior that handles infinite uncertainty but that is still manageable my the EKF.

Figure 1 :

 1 Figure 1: Point parameterizations. Image extracted from [?] 1) Euclidean point: The parameters of an Euclidean point consist on its Cartesian coordinates.

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two a = [a a] T and b = [b b]

Figure 3 :

 3 Figure 3: Plücker line geometrical representation.

  and e 1 , e 2 , n C are mutually orthogonal.

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two points = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

re 2 :

 2 Plücker line geometrical representation. Image cted from [?] lücker coordinates transformation from camera e is performed as shown next:

Figure 4 :

 4 Figure 4: Plücker line back-projection.

Figure 5

 5 Figure 5 shows this parameterization.

Figure 1 :

 1 Figure 1: Point parameterizations. Image extracted from [?] 1) Euclidean point: The parameters of an Euclidean point consist on its Cartesian coordinates.

d

  This allows to express the unbounded distance of a point along the optical ray from 0 to infinity, into this bounded interval in parameter space ρ ∈ (0, m/d min ].

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical represent extracted from [?]

1 )

 1 Plücker line: A line in P 3 defined by two points a = [a a] T and b = [b b]

Figure 2 :

 2 Figure 2: Plücker line geometrical representation. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 :

 4 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

Figure 3 :

 3 Figure 3: Plücker line back-projection. Image extracted from [?]

Figure 4

 4 Figure 4 shows this parameterization.

Figure 4 : 3 Figure 5 :

 435 Figure 4: Anchored homogeneous-points line parameterization. Image extracted from [?]

2 ρ

 2 d min Being n = 1, leads to, ρ = 1/3d min , and σ ρ = 1/3d min The probability distribution function of a point supported line is defined as t C ∼ N t; T , with: Plücker lines prior β C ∼ N β; B take the following values:

1 )

 1 Identify mapped magnitudes x ∼ N {x, P}.2) Identify measurements z ∼ N {z, R}, where z is either point or line (i.e. u or s respectively). 3) Define Gaussian prior π ∼ N {π; Π} for unmeasured degree of freedom. π can either be ρ C , t C or β C . 4) Back-project the Gaussian measurement and get landmark mean and Jacobians. L = g C, z, πG C = dg dC C,z,π , G z = dg dz C,z,π , G π = dg dπ C,z,π5) Compute landmarks co-and cross-variances.P LL = G C P CC G T C + G z RG z T + G π ΠG T π P Lx = G C P Cx = G C [P CC P CM ]

z 2

 2 = h2e(u) = u 1 /u 3 u 2 /u 3 ∈ RInnovation y is then computed as Innovation mean: y = zh (x)Innovation covariance: Y = R + H • P • H Twhere R = U the measurement noise covariance and Jacobian H = ∂h ∂x x.

Figure 6 :

 6 Figure 6: Environment world of the simulation experiments.

Figure 7 :

 7 Figure 7: Sensor view considering transparent objects.

Figure 8 :

 8 Figure 8: Sensor view considering opaque objects.

Figure 9 :

 9 Figure 9: Image of the environment after performing a complete turn with Plücker line + inverse depth point parameterization.

Figure 10 :

 10 Figure 10: Transparent objects with loop acquaintance position estimation errors.

Figure 11 :

 11 Figure 11: Transparent objects without loop acquaintance position estimation errors.

Figure 12 :

 12 Figure 12: Opaque objects with loop acquaintance position estimation errors.

Figure 13 :

 13 Figure 13: Opaque objects without loop acquaintance position estimation errors.

Figure 14 :

 14 Figure 14: Some frames of the sequence are included and the detected/tracked segments are shown in green, blue and yellow, while detected points are displayed in red.

ACKNOWLEDGMENT

This work was performed during a long-term stay of J.O.Esparza Jimenez, PHd student in ITESM Monterrey, Mexico, in the robotics department of LAAS-CNRS in