Visual EKF-SLAM from Heterogeneous Landmarks.

sensors Article

Visual EKF-SLAM from Heterogeneous Landmarks † Jorge Othón Esparza-Jiménez 1, *, Michel Devy 2,‡ and José L. Gordillo 1,‡ 1 2

* †

‡

Center for Robotics and Intelligent Systems, Tecnológico de Monterrey, Monterrey 64849, Mexico; [email protected] CNRS, LAAS, Université de Toulouse, 7 Avenue du Colonel Roche, Toulouse F-31400, France; [email protected] Correspondence: [email protected]; Tel.: +52-81-8358-2000 (ext. 5141) This paper is an extended version of our paper published in Esparza-Jimenez, J.O.; Devy, M.; Gordillo, J.L. EKF-based SLAM fusing heterogeneous landmarks. In Proceedings of the IEEE 2014 17th International Conference on Information Fusion (FUSION), Salamanca, Spain, 7–10 July 2014; pp. 1–8 These authors contributed equally to this paper.

Academic Editors: Lianqing Liu and Yajing Shen Received: 31 December 2015; Accepted: 30 March 2016; Published: 7 April 2016

Abstract: Many applications require the localization of a moving object, e.g., a robot, using sensory data acquired from embedded devices. Simultaneous localization and mapping from vision performs both the spatial and temporal fusion of these data on a map when a camera moves in an unknown environment. Such a SLAM process executes two interleaved functions: the front-end detects and tracks features from images, while the back-end interprets features as landmark observations and estimates both the landmarks and the robot positions with respect to a selected reference frame. This paper describes a complete visual SLAM solution, combining both point and line landmarks on a single map. The proposed method has an impact on both the back-end and the front-end. The contributions comprehend the use of heterogeneous landmark-based EKF-SLAM (the management of a map composed of both point and line landmarks); from this perspective, the comparison between landmark parametrizations and the evaluation of how the heterogeneity improves the accuracy on the camera localization, the development of a front-end active-search process for linear landmarks integrated into SLAM and the experimentation methodology. Keywords: SLAM; EKF; computer vision; landmarks; points; lines

1. Introduction Simultaneous localization and mapping (SLAM) is an essential functionality required on a moving object for many applications where the localization or the motion estimation of this object must be determined from sensory data acquired by embedded sensors. The object is typically a robot or a vehicle, the position of which is required to deal with robust navigation in a cluttered environment. A SLAM module could also be required on smart tools (phones, glasses) to offer new services, e.g., augmented reality [1–3]. The robot or smart tool could be equipped with a global navigation satellite system (GNSS) receiver for outdoor applications to obtain directly a position with respect to the Earth reference frame [4]; at present, indoor localization with respect to a building reference frame could also be provided using ultra-wide band (UWB) [5], WiFi [6] or RF devices [7], on the condition that a hotspot or antenna network has been previously installed and calibrated. However, the direct localization is not always available (i.e., occlusions, bad propagation, multiple paths); so generally, they are combined using loose or tie fusion strategies, with motion estimates provided by an inertial measurement unit (IMU), integrating successive accelerometer and gyro data [8–10]. Nevertheless, even GPS-IMU fusion could fail or be too inaccurate. Depending on the context, a priori knowledge could be exploited; Sensors 2016, 16, 489; doi:10.3390/s16040489

www.mdpi.com/journal/sensors

Sensors 2016, 16, 489

2 of 25

a map matching function can be sufficient, as in the GPS-based navigation systems available on commercial vehicles. Considering mobile robots or the emerging autonomous vehicles, it is necessary to also make use of data acquired on the environment with embedded exteroceptive sensors, e.g., laser range finders [11], 3D sensors (ToF cameras [12], Kinect [13]) or vision with many possible modalities (mono, stereo, omni). Here, only visual SLAM is considered, due to the fact that it could be integrated both in low-cost unmanned ground and aerial vehicles and on smart tools equipped with cameras. Many visual SLAM methods have been proposed during the last decade [3,14]. A SLAM method combines two interleaved functionalities shown in Figure 1: the front-end detects and tracks features from images acquired from the moving robot, while the back-end, interpreting these feature and landmark observations, estimates both the landmark and robot positions with respect to the selected reference frame. SLAM% % Back+end% % % % Landmark% % % ini3aliza3on% % % % % % % % % Map%update% % % %

Front+end% % Landmark% % detec3on% % % % Landmark% % tracking% %

Figure 1. General block diagram of the SLAM solution. The blocks in gray represent the contributions of the present method.

The back-end can be based either on estimation (Kalman [15], information [16], particle filters [17]) or optimization (bundle adjustment) frameworks [18]. The more classic landmarks are 3D points, detected as interest points (SIFT [19], SURF [20], FAST [21]), matched by using their descriptors (binary robust independent elementary features (BRIEF) [22]), or tracked by using Kanade-Lucas-Tomasi (KLT) feature tracker [23], or an active-search strategy [24]. Generally, the set of 3D points extracted from an image does not give any semantic information, unlike 3D lines, which correspond generally to sharp 3D edges in the environment. This is the reason why segment-based SLAM from either an estimation [25] or optimization [26] back-end, has been proposed. The main challenge of these methods concerns the front-end, i.e., the robustness of the line detection and tracking in successive images. The initialization of such landmarks with their minimal Euclidean parameters requires more than one observation. One way to solve this problem was the delayed initialization [3,27], in which a landmark was added to the map only when it was known in the Euclidean space. This does not allow use of landmarks that are very far from the robot. An alternative solution is to add them to the map, as soon as they are observed (i.e., undelayed initialization), and it has been proposed for point [28,29] or line [25] landmarks. The pros and cons of several representations for 3D points and 3D lines have been analyzed in [30]. This article is devoted to the analysis of a visual SLAM solution using a heterogeneous map, as a more complete approach where points and lines are both included from features extracted in images acquired by a camera moving in the environment, and with undelayed initialization. Therefore, the contributions of the proposed method comprehend the use of heterogeneous landmark-based SLAM (the management of a map composed of heterogeneous landmarks), and from this perspective, the comparison between landmark parametrizations, the development of a front-end active-search process for linear landmarks integrated into SLAM (the processing of linear landmarks by the

Sensors 2016, 16, 489

3 of 25

inclusion of detection and tracking algorithms taken from the state of the art) and the experimentation methodology. These contributions correspond to the gray blocks in Figure 1. Only the first two contributions were covered in [31], which dealt with the information fusion back-end for the map computation. The present article extends this concept to its application on real images, by developing the proposed front-end. Even if optimization-based methods make it possible to avoid the possible divergence of methods based on estimation due to linearization of the observation model, the fusion is performed from an extended Kalman filter, as a very light approach that can be integrated into a dedicated architecture to be used on small aerial vehicles. Section 2 covers the techniques and parametrizations for the initialization and update of point and line landmarks on the map. Section 3 describes the detection and tracking methods selected and developed for the front-end. Section 4 focuses on experiments, including a simulation part that compares landmark representations, a real image part that recalls the integration of segments with the already existing point-based front-end and an integral experiment. The results of the experiments are C −1 presented and discussed in Section 5. Finally, Section 6 offers some conclusions.m = K u. The complete homogeneous point parameterization is given in the following equations:

M with heterogeneous landmarks The SLAM back-end deals with the initialization of observed features as landmarks on the heterogeneous landmarks map and the estimation or update of both the landmarks and the robot positions with respect to a selected reference frame. This section deepens the description of each landmark parametrization, EKF-SLAM with heterogeneous landmarks the initialization and update algorithms and it derives from the theoretical study presented in [31]. terogeneous landmarks Devy Jorge Othón Esparza-Jiménez The undelayed landmarkheterogeneous initialization (ULI) is presented in [30] for different point and line EKF-SLAM with landmarks � � Jorge Oth´ on Esparza-JiméThe nezmain idea α 0 u parametrizations. is to model the uncertainty derived from unmeasured degrees of ogeneous landmarks � � Michel Devy K = 0 α v Jorge Othón Esparza-Jiménez 2. SLAM Back-End

wh having

Ge the pl v is t � � � −1 � K u m orthog LHP = p = =H = C ρ ρ byab�n− mC = K−1 u. where n = a × b, n = � � (5) −1 C to cons ρ o RKhaving u +the Tρfollowing Plücker The complete homogeneous point parameterization repres ρC is given in the following equations: Geometrically speaking, n is , the plane π containing the line u 0 C must be given asv prior and represents isby theEKF. director vector from a � in�a where � ρ −1 �can v unbounded 0 freedom by a Gaussian limits, manner that be origin handled αu 0 prior u0 that handles from the of coordinates. K u m inverse-distance orthogonal distance from the line � � 0 0 1 L = p = = H = HP C0 K implications = 0 ρu CTION 0 The are different for point and line For points, depth is Thus, �v� is the inv ρ α u landmarks. Abstract— Jorge Oth´ on Esparza-Jim´ e0nezαv of vuncertainty by �n�/�v�. Anchored � e3) � (5) point: In order to im0nez αv infinity. v homogeneous K = ray 0andand 0T uncertainty 1 Michel Jorge Oth´ onthe Esparza-Jim´ −1 RDevy are matrixalong and translation toofρinfinite of ahomogeneous unknown, is present visual until In the iscase RK u +10TρanC anchor � the rotation prove linearity, added as reference topoints. Plü AMETERIZATION initially � 0 0 α 0 u C � I. I NTRODUCTION representation is shown in figure u 0 � vector that define the camera C. Underlined vectors like optical timeshould of the landmark. R and T areelines, the rotation matrix and translation straight uncertainty is present in two degrees of freedom, a depth that αthe 0 uρ0center attoinitialization orge Oth´ on Esparza-Jim´ nez u corresponding Abstract— 0 α v K = an undelayed landmark R and T are the rotation matrix and translation v 0 u represent homogeneous coordinates. Thus, the landmark is a 6-vector that includes the anchor , II. L ANDMARK PARAMETERIZATION vector that theup camera vectors like 0 αv v0 K= be covered and all possible orientations. �define � C.1Underlined vector that define the03D camera C.1Underlined vectors like 0 uto 0infinity tmark points and paramcoordinates, the Cartesian coordinates of the point α 0 C 0 u lines represent homogeneous coordinates. where ρ must be are given priortoand I. auIetNTRODUCTION In [?], Sol` al. introduce an undelayed landmark 2)0Homogeneous point: Homogeneous points unez represent homogeneous coordinates. with as respect the represents anchor, and an inverse-depth scalar. stituting the unmeasured Devy Jorge Oth´ o n Esparza-Jim´ e aram-Michel 0 α v K = v 0 R and T2.1. are(ULI) rotation matrix andlines translation inverse-distance origin ofmatrix coordinates. for different points param3Dthe Point Parametrizations R T �arefrom thethe rotation conformed by a and 4-vector, which is and composed by the � and � translation 2) Homogeneous point: Homogeneous points sian prior initialization that handles II. L ANDMARK PARAMETERIZATION � are sured 2) u Homogeneous point: Homogeneous points p0 vectors 03D 1 substituting eterizations. It0the consists on the unmeasured vector that define camera C. Underlined vectors like α 0 vector that define the camera C. Underlined likeare u 0 vector m and scalar ρ. T 7 conformed byIna this 4-vector, which is composed byis the 3) Anchored by homogeneous point: Inis order to immy the conformed areference, 4-vector, which composed byz0the m ndlesmanageable L = = [x y mx mof AHP 0 0 y m z ρ] ∈ R . of by section, acoordinates. Gaussian prior that handles a Euclidean point described as a for subsequent covering the kstill In degree [?], Sol`homogeneous a etfreedon al. introduce an undelayed landmark 0 α v K = u represent u represent homogeneous coordinates. � � v 0 prove linearity, an anchor is added as a reference to Rtheand T are the rotation matrix and translation 3D vector m and scalar ρ. 3D vector m and scalar ρ. ρ y infinite uncertainty but that is still manageable my the mpurposes (ULI) for points and lines param- (i.e., 0 homogeneous 0 T1center3 at�initialization - I. Iinitialization 4 � and time ones used initialization anchored homogeneous point). Figure NTRODUCTION � different �HP the optical of the landmark. L =p= = [mx m ρ] ∈ P ⊂ point Rpoint: ctor that define the camera C.for Underlined vectors y m zHomogeneous 2) Homogeneous points are EKF. 2) Homogeneous point: Homogeneous points are eterizations. It consists on substituting the unmeasured ρ Tlike m dpes such The convertion from anchred homogeneous point to T m as points and 3 4 extrac Thus, the landmark is a 6-vector that includes the anchor 3 4 and Each description includes camera projection back-projection. R and T are the rotation matrix and translation L = p = = [m m m ρ] ∈ P ⊂ R conformed by a 4-vector, which is composed by the represent homogeneous coordinates. HP x y z L = p = = [m m m ρ] ∈ P ⊂ R NDMARK PARAMETERIZATION degree ofFor freedon by landmark aρ Gaussian that HP xprior y composed z handlesby the conformed by a 4-vector, which issuch ρ Euclidean coordinates can be achieved by the following For different types asdefine pointstheand points, 3D coordinates, the Cartesian coordinates of the point vector that camera C. Underlined vectors like ssmplications. andinfinite uncertainty 3D vector mare scalar in ρ. Figure 2. Instill order to convert from toand Euclidean Point parametrizations used for homogeneous initialization shown but that is manageable my the 3Dit vector ρ. lines, uncertainty has distict implications. For with respect to �the anchor, and inverse-depthtoscalar. eoints, al. introduce an m undelayed landmark et 2) and covers alland thescalar � equation: Homogeneous point: Homogeneous points are points, u represent homogeneous coordinates. In order to convert fromanhomogeneous Euclidean coordinates, the following equation is applied: EKF. there � � In order to convert from homogeneous to Euclidean ishandles uncertainty in distance and it by covers all the T �= the �m following Plu 3 LI) for different points and m lines paramte straight lines coordinates, equation is applied: onformed by a 4-vector, which is composed the ll the L = p = [m m m ρ] ∈ P ⊂ R4 m p HP x y z 0 ρ points are 3handles 4 coordinates, the following equation isT lines applied: 2)as Homogeneous point: Homogeneous visual ray until infinity. Infinite straight frame Tp = p onsists on substituting the unmeasured For different landmark types such points and L = p = = [m m m ρ] ∈ P ⊂ R 7 HP x y z freedom, which corre+ . (6) D vector muncertainty and scalarinρ. ndles m = [x0by y0 the z0 mx my mz ρ] ∈ R .0 ρtwo m LAHP by a 4-vector, which= is composed degrees of conformed freedom, corredon bylines, a Gaussian prior that handles uncertainty has distict implications. Forwhich points, m homogeneous to Euclidean ρ �distance pinfinity, =scalar. ρ. In order to ρconvertp (2) be covered up�to to infinity, from correZ = , d = �m�/ρ. (2) spond a that should be covered up to 3D vector m and m ,tythe butnext thatInsections, isorder still my thehomogeneous there uncertainty in distance andT it covers all the ρ coordinates, the following ρequation Figure 2: Plücker line geometricalL mmanageable topoint convert from to 3 sections, 4 Euclidean is applied: nfinity, � point � p = , d = �m�/ρ. (2) and all possible orientations. In the next L = p = = [m m m ρ] ∈ P ⊂ R The projection and frame transformation process The convertion from anchred homogeneous point to HP x y z Z visual ray until infinity. Infinite straight lines handles extracted from [?] is e for ULI ρ ρ m T coordinates, the following equation is applied: 3 given 4 in the next expression: areand covered, for point line parameterizations used for ULI are covered, for=Euclidean L = p = [m m m ρ] ∈ P ⊂ R coordinates can be achieved by the following HP x y z uncertainty in two degrees of freedom, which corresn landmark In the camera frame, mρ is the director vector offrame, the m is the director vector of the In the camera Th types such as points and process then deepen in the initialization process d,In fororder equation: p a=linear . dependence (2)− p ) ρ) ∈ P2 . sponditself. to convert a distance that should bem covered toitself. infinity, to from homogeneous to up Euclidean Y optical ray, andinverse ρ has to the inverse optical ray, ρ has a linear dependence to the u = KRT (m − (T (7) Plücke In the camera is and the director vector of the y- has distict implications. Forframe, points,m 0 ρ Insections, topoint convert from of homogeneous Y allthe possible orientations. In is the oordinates, following equation applied: the distance defined the optical center to the Plücker coordinates transform p the . next (2) ,nty inand of distance dorder defined from the optical centertodtoEuclidean the from distance and it and covers all the optical ray, ρ has a= linear dependence the inverse A. parameterizations 3D point parameterizations m coordinates, thetofor following equation is applied: and line used for ULI are covered, ρ frame is performed as shown nex point. p = p + . (6)homogeneous point. tinfinity. Infinite straight lines handles 0 The complete anchores point paramIn the camera frame, m is the director vector of the of the distance d defined process from the optical center to the then deepen insection the initialization itself. ρ � This explains some point parameterizations. �m� is the following: m correwo degrees of freedom, which eterization parameterizations. optical ray, and ρ has a linear dependence to the inverse rpoint point. R [T] ρ= d �m� m C The aspects in each description refer toof the p included =frame, .Om (2) In the camera is the director vector the ρ =   L = H · L = P L p = .O (2) ce that should be covered up to infinity, distance d defined from the optical center to�isthe PL � description refer parameterizations to the ρitself, camera d of the tions. 0 R The projection and frame transformation process A. 3D point parameterization projection, coordinate �m� T p ρ 0 This allows to express the unbounded distance −1 has a linear to the inverse wh Inray, the and next ρsections, point ρ =dependence point. aorientations. coordinate o projection, theopticaltransformation, d   given in the next expression: and back-projection. This allows expressto the distance Lray = 0m = RK u , (8) AHP of a point along from to infinity, X to center define erizations useddistance for ULIexplains are covered, for This section some pointthe parameterizations. Xthe optical of the d defined from optical the unbounded The whole transformation and ction. �m� dinate In the camera frame, m in istoof theexpress director vector of to the ρ In the camera frame, m is thethis director vector of the ρC coordinates in terms of R a point along the optical ray from 0 to infinity, T 2 ρ = This allows the unbounded distance into bounded interval in parameter space ρ ∈ The aspects included each description refer the he initialization process itself. u = KR (m − (T − p ) ρ) ∈ P . (7) Pl¨ u cker d point. 0 optical and0ρ to hasinfinity, a linear dependence ptical parameterization ray, of andaρ point has a itself, linear dependence to theray, inverse (b) into this bounded interval in parameter space ρ inverse ∈ (0, �m�/d ]. the along camera the optical ray(a) from minto projection, coordinate . Thistheallows tocenter express the unbounded distance �m� of center C the distance d defined from optical to the the distance d defined from the optical to the (0, �m�/d ]. where given as prior. into this bounded interval in parameter space ρ ∈ transformation, and back-projection. min ρ = complete homogeneous point eameterizations The frameanchores transformation anmust homogeneous point of aThe point along the optical rayofρfrom 0 be toparaminfinity, d point. l = K · RT · (n − T oint. (0, �m�/d ]. min eterization is the following: is performed tointheparameter next equation: into this boundedaccording interval space ρ ∈ eexplains some parameterizations. The frame transformation of an homogeneous point Thispoint allows to express the unbounded distance �m�  line parameterizations  W Figure 2. Point parametrizations used for initialization. point parametrization; B. �(a) Homogeneous � 3D �m�/d ρ(0, = �m� min ]. uded of in each description refer the rayof from d isto performed according the next equation: The along frame transformation an homogeneous point p0 ρ= � T � a point theanchored 0point totoinfinity, frame, d opticalhomogeneous where K is the instrinsic proje (b) parametrization. −1 itself, camera projection,according R section, Tu  C,some line C=In parameterizations are covRK Lunbounded =p =m (8) performed next transformation an homogeneous point Thisequation: allows to ρexpress theframe distance AHP into isthis bounded coordinate interval tointheparameter space ∈ The Hp =ofthis p , (3) defined as: �the optical � ray from C1 0 ρdescription ndThis back-projection. allows to express the unbounded distance ered. The of projection to image frame, ρ is performed according to the next equation: of a point along 0 to infinity, � bi(0, �m�/d ]. parameterizations. R T C C Figuremin 1: extracted � into αv 0 a point along thePoint optical ray from 0Image pto =�infinity, Hp = from , (3)linear this bounded interval in pparameter space ρ ∈ transformation and back-projection are included. 0 1 R �m�/d T [?] frameinterval � � the frame to which C C ].∈ 0 α K = where super-index C indicates u C to this bounded in parameter space ρ (0, 3 The transformation of an homogeneous point (3) where ρ must be given asRprior. PlüTcker line: A line P defined by two points C ns. Image extracted fromp = Hp = 0 1 pmin, −αv u0 αu v0 the point p is = refered, and 1) matrix H psepecifies thein(3) frame HpC = , T T , �m�/d is performed according to the next The equation: min ]. Lin 1 and b = [b b] can be represented as =0 [a a] frame C transformation ofwhich an homogeneous point from thetopoint is atransformed. where super-index indicates to the frame which measu 1) Euclidean point: The parameters of an Euclidean Figure 1: Point parameterizations. Image extracted from When Pl¨ u cker coordinates are homogeneous 6-vector, known as Pl¨ u cker coordinates: B. the 3D next line equation: parameterizations is refered, performed according to The[?] framepoint transformation anpoint � � the point the is and matrix H sepecifies the frame where super-index Chomogeneous indicates frame to which � image consist on its of Cartesian coordinates. Thesuper-index projection of indicates a point �into the frame is frame, projection is only obtained where C the to which R T n frameare Fo C is transformed. performedtheaccording toHp theCto next equation: T In this section, some line parameterizations covwhich the point point p is = refered, and matrix H sepecifies the frame � � performed with the following expression: T p 3, = (3) L = = [n n n v v y v z ] ∈ P5 ⊂ R 6 , the point is refered, and matrix H sepecifies the frame P L x y z x rameters of an EuclideanLEP = p = [x 0 y z]1 ∈ R are co TheTdescription of projection to vimage frame, biC ered. R C to which the point is transformed. p = Hp to = which the point p , is transformed. (3)

-SLAM with heterogeneous landmarks

Sensors 2016, 16, 489

4 of 25

2.1.1. Euclidean Point A Euclidean point is parametrized by its Cartesian coordinates.

L EP = p = [ x y z]T ∈ R3 The projection to the camera frame is performed as follows: u = KRT (p − T) ∈ P2 where,



0 αv 0

αu  K= 0 0

(1)

 u0  v0  1

is the intrinsic parameter matrix and R and T are the rotation matrix and translation vector that define the camera C. Homogeneous coordinates are represented by underlined vectors, like u. 2.1.2. Homogeneous Point Homogeneous points are four-vector composed of the 3D vector m and scalar ρ, as introduced in [32]. " # T m L HP = p = = m x my mz ρ ∈ P3 ⊂ R4 ρ

The vector m gives the direction from the origin O to the point p, while ρ serves as a scale factor for providing the magnitude for each coordinate of the point. The conversion from homogeneous to Euclidean coordinates is given by the following equation: p=

m ρ

(2)

Depending on the characteristics of the parameters m and ρ, there are three different canonical representations for a homogeneous point. The original Euclidean point refers to the case when ρ = 1, inverse-depth has mz = 1 and in inverse-distance kmk = 1. In the camera frame, m is the director vector of the optical ray, and ρ has a linear dependence on the inverse of the distance d defined from the optical center to the point: ρ=

kmk d

The unbounded distance of a point along the optical ray from zero to infinity can then be expressed in the bounded interval in parameter space ρ ∈ (0, kmk/dmin ]. The frame transformation of a homogeneous point is performed according to the next equation: C

p = Hp =

"

R 0

T 1

#

pC

(3)

where super-index C indicates the frame to which the point is referred and matrix H specifies the frame to which the point is transformed. The projection of a homogeneous point into the image frame is performed with the following equation: u = KRT (m − Tρ) ∈ P2

(4)

Sensors 2016, 16, 489

5 of 25

By expressing a homogeneous point in the camera frame, the projected image point is u = KmC , where super-index C indicates the frame to which the point is referred. In this case, ρC is not measurable. Back-projection is then: m C = K −1 u The complete homogeneous point parametrization is the following:

L HP = p =

"

m ρ

#

=H

"

K −1 u ρC

#

=

"

RK−1 u + TρC ρC

#

(5)

where ρC must be given as a prior and represents the inverse-distance from the origin of the coordinates, that is the scalar value that makes kmk = 1. 2.1.3. Anchored Homogeneous Point Linearity is supposed to be improved by the addition of an anchor that serves as a reference to the optical center at the initialization time of the landmark. The landmark is then composed of seven elements that include Cartesian coordinates of the anchor, the point with respect to the anchor and an inverse-distance scalar.   p0 T   L AHP =  m  = x0 y0 z0 m x my mz ρ ∈ R7 ρ The conversion from the anchored homogeneous point to Euclidean coordinates can be obtained by the following equation: m p = p0 + (6) ρ The projection and frame transformation process is given below: u = KRT (m − (T − p0 ) ρ) ∈ P2

(7)

The anchor is chosen to be the position of the optical center at the initialization time, given by T. That way, the term multiplying the unmeasured degree of freedom ρ (i.e., (T − p0 ) ρ) is small after initialization. This helps to decouple the uncertainty of the most uncertain parameter ρ. The complete anchored homogeneous point parametrization for back projection and transformation is the following:

L AHP where ρC must be given as the prior.



   p0 T     =  m  =  RK−1 u  ρ ρC

(8)

2.2. 3D Line Parametrizations The line parametrization includes the projection to the image frame and back-projection to 3D. The Plücker line and anchored homogeneous point line are shown in Figure 3.

way of points. representing a line is (11) by homogeneous the Cendpoints that HP L representation define it.from from anchored  1  is shown in fig Othón Esparza-Jim´ enez −1homogeneous ρ ofextracted homogeneous u2cker the line geometrical C anchred 1 The convertion from point by toto the C n =Departing K · l Pl¨ ρ RK u +lcan TρCbe achieved [?]  m  = K (11) Defining β point = (β βparam, vector cananchored be also⇤define Euclidean coordinates following from the anchored homogeneous 2 ) ∈ R an the anchor, and an The homogeneous ogeneous point: In inverse-depth order to im- scalar. vC n= Figure e2:complete +Cu·cker · e2 anchores (12) geometrical representation. it.1 , Departing fromv the homogeneous 1 Pl¨ pointdefine parameterization, line isit.anDeparting 2 T representation is shown figure 2. homogeneous-point CImage ρ 2 byline Euclidean coordinates can = be1 ·achieved the following �a reference �homogeneous C l = K · R · (n − T × v) vas: = βin (12) expressed 1 · e1 + β 2 · e2 point parameterization, an homogeneous-point line (9) is an equation: � Pl¨ , The convertion from anchred point to chor is uadded as to ρ2 π v · e + · e (12) extracted from [?] 1 is1 the 2following: 2 pointdefined parameterization, 11-vector as follows:an homogeneous-point line is an cker coordinates from eterization camera αu 0 utransformation 0 equation: , 0 ⇧ ⌃ 11-vectorfrom defined as follows: ⌦ transformation Euclidean coordinates can by the following nitialization time of the landmark. T be achieved   C Pl¨ u cker coordinates camera C as 11-vector defined follows: � � m = [x0isK y0performed z= ρ]v shown ∈ vR07 . next: frame   � � α For each point, the transformation and projection o where , ⇧ R and e , e , n are mutually 0 mx m0y mzas p where ⇤ must be given as prior and represents 2 1 ⇧2 C 0 Michel Devy Jorge Oth´ on Esparza-Jim´ eand nez ⌃ equation: a Figure 6-vector thatPl¨ includes m ⌦ (13)  C  where βPl¨ , uβcker e1E ,performed e·2β, ,n areasmutually where ρC 1must beImage given as prior and represents ppreviously 2: ucker the lineanchor geometrical representation. 1T 2 ∈ Rcoordinates 0 transformation from camera frame is shown next: 0 mutually vC = a pinhole is where K is the instrinsic Pl¨ cker matrix where R and e1=, ep20, n 2) Anchored Homogeneous-points line: Another p +C p.are (6) orthogonal. m1the �Plü� �geometrical p inverse-distance from origin of coordinates. n camera projection RK,as1line: lm + T⇥ REustated, 1 , line 2 ⇧ 0 the point 0 � 1 inverse-distance 02) Anchored Cartesian coordinates of −1 ⇤ ⌅ from the origin of coordinates. orthogonal. Figure 2: cker representation. Image Homogeneous-points Another m 2) Anchored Homogeneous-points line: Another C   L = H = , ( Pl¨ u cker coordinates transformation from camera 1 PL RK ⇥ vC C ⇥ 11 extracted from [?] CAbstract— isuperformed as shown next: np = p1 0 + L. AHP = ⇤m (6)=wayframe uL0(8) of representing is by⇤1defined theway endpoints that  endpoints  orthogonal. m⇧ LaHPline = u , 0 11 as: × R from 1R ion anchred homogeneous pointRto m [T] chor,from and anL inverse-depth scalar. extracted [?] of representing a line 2)CCAnchored Homogeneous-points Another ρRE Lis =the [T]11is Rbyorder that nto LP =H · LP L =p . )· ⇧ frame is asC1 to shown next: way representing aR line the endpoints 1  ∈ Rthat nC· homogeneous = l point: (11) CDeparting HPby Lline: C −1 3) Anchored homogeneous point: In im↵ 2) Anchored Homogeneous-points line: Another ⇥ C1K C2performed 2vof C C Defining = ( , R , vector v can be also p = + . (6) ⇤ ρ 3) Anchored In order imdefine it. from the anchored homogeneous ⇥ ⇥ ρ 2 n = K · l (11) 0 T 0 nd T are the rotation matrix and translation L = H · L = · . K =   0 R m Defining (β )∈ RP L ,L vβ.of also ⇤be v where v=(11) ∈ 1π, β2for value of= ⇧Departing RR vany 0vector n = K · l� C dinates can be achieved by the following 2Pcan 1 from Chomogeneous HP L L � where must provided as prior. define it. from the anchored homogeneous uathe = KR (m (T −�p0 ) ρi ) (17 m ρ prove way representing aCan line is vby endpoints 2i − that �process � βCof define Departing anchored ito is� representing 1be prove linearity, an is 0 added as a that reference vC an=The + ·as ait.line is↵ by endpoints R⇤anchor [T] R C point nbe Defining = projection (1 · ise,1added Re22a,frame vector v linearity, anchor reference tocan expressed as: 2 and parameterization, an 1homogeneous-point linethe is 0 21)C⇧ C expressed as: vhomogeneous-point nC transformation =(12) K ·(11) l also (11) ⇥the m v = way β ·LeP += β02 H · e2·0 (12) R α  line T Sensors hat the camera C. like 2 it.point 2 parameterization, 1n 1(12) ananchored ρ2 0line is an 7 Underlined nCinitialization =the1 Knext · expression: l= = L = define · and . the C1 · e1 +R 2 [T] v e×2·11-vector I. optical Ivectors NTRODUCTION 2016, 16, 489center 6 of 25 Departing from homogeneous L C 1is point parameterization, anhomogeneous homogeneous-point an Cof the [x0 ydefine z m m m ρ] ∈ R . given in P Lexactly L H · L = · . the optical center at initialization time landmark. the at time of the landmark. define it. Departing from the anchored 0 0 x y z In order for �β� to be inverse-distance defined as follows: expressed as: n RK l + T ⇥ RE P L C C 0 R P L 2.2.2. Anchored Homogeneous-points line v 0homogeneous α whole transformation and process for Kprojection Theprojection projection and frame transformation process ⇤2 follows: 0 1 ·isRe1 + 2 · e 11-vector defined follows: vthe C camera ⇧ transformation The whole transformation and projection forutransformation The projection and frame transformation process is L= =ashomogeneous-point HAn = ,. Plülandmark cker coordinates camera v2from =given (12) 2D line0isisanobtained by the(10) cros Pl¨ ucker coordinates transformation from ⌦ sent The homogeneous coordinates. P Lan 2 point line Cprocess For each the transformation and of defined vis = where ·, ee21 ,includes ·are ethe (12) R and are rotation matrix ρ+nCC ⌃2must be as Thus, the 6-vector that anchor �parameterization, �11-vector e be parallel toThus, thepoint, image plane, eand e6-vector are obtained For each point, the and projection of the landmark isparameterization, atranslation that includes the anchor point homogeneous-point line is an v RE 1 and 2 as α T 2prior. where ,next ⇧and Ra and e11representation. mutually A can be represented by the points it. Apply m pan II. LFigure ANDMARK ⇧ ⌃∈T 2 line 2: u1cker geometrical Image Pl¨ ucker coordinates terms of T, n, vas −α v0end αuonαit,vthat given in ,Pl¨ the expression: in ⌦product Cu frame isPARAMETERIZATION performed shown CCucker = KR p0β) 1⇤) ⇧ .1R aand (7) Pl¨ coordinates in line terms ofalso ,pinhole T, n, and v0of is: two points lying l = udefine vofup 0 uas where ,that β ecamera e3D ,vn are mutually defined aspRand follows: 2C P 1 ,expressions: 2camera 1 × u2 and thus p given = is phomogeneous + the. next expression: (6) pinhole is ,m as011-vector previously stated, 3D R coordinates, the Cartesian coordinates of(T the point frame asinshown next: For each point, the transformation projection vis: = E ·next: , (m (13) =E · β, (13) with the next a camera is , previously stated, vector define the C. Underlined vectors like 0 performed coordinates, the Cartesian coordinates of the point 11-vector defined as follows: where , ⇧ R and e , e , n are mutually The whole transformation and projection process for orthogonal. the anchored homogeneous point parameterization, an homogeneous-p The whole transformation and projection process for 1 2 1 2 m anchred point to ⇧ ⌃ where must 0be provided⌦ asma prior. 1 extracted from [?] � � � � C ⇧ ⌃ orthogonal. Homogeneous ρupoint: Homogeneous points are  C T 2 ⌦ with thean and inverse-depth scalar. 1 In(m [?], Sol` al. respect introduce undelayed landmark 11 v = E,)e·⇤) (13) Tanchor, 2⇧ av pinhole camera is , as previously stated, where and , en, n are mutually � RPl¨ uCRrepresent homogeneous coordinates. p0 as respect anPl¨ inverse-depth KR −�(T −a pet Pu�. =to�KR (7) u(T cker coordinates in of R ,eT, is:with orthogonal. line isWhen defined follows: 1,n 1 2 ,uand ⇤terms m  scalar.  are LHP = in to ⇧ 0 ) ρ) ∈ Ran can be achieved by=the following nterms 1 thepanchor, 1v L where ⇧ and e[T] mutually (m p ⇧, C·P2homogeneous .are Pl¨ cker coordinates ofR R ,anand T,11-vector n, and ucker coordinates in camera × 2R, C 2L 10 parameterizations 0 2 line C ρ1 line LHPis: = 11 ∈T R11 expressed �points = [p T LP�Lby =1(,H =3D . (7) point anchores paramLm ↵⇤ m Defining = ,C CP2B. )complete ⇧ R , vector med byframe a 4-vector, is iscomposed the C [T] Rdifferent n C also initialization for and lines paramL 1·The 2.2.2. C TAnchored⇤Homogeneous-points orthogonal. ⌅ ⇢1(m m2 − ⇢2 ](T 2−Rp11) ρ ) Cwhich p0⇧ where π,Cvector for �anyvCvalue of 1 0 Rofv . can T R(ULI) AHP L 0 m L ⇧ Ronly × vbe 2β. 1(9) Tu = Defining βHomogeneous = (β R2point: can be also on and L transformation process 1⇤ L where v ⇥ for any value orthogonal. HP L ·= KR l = K · R (n T v) ⇥ m � 1 , β2 )v∈ ∈ i by 1 = H · L = · . p 2 C T m i l = KR ((m (T −that p0 ) 0define × (ρi 1 m − ρ2(17) m1 )) frame, projection is obtained l = K · R · (n − T × v) π (9) T T 2 2) Homogeneous points are 0 P L 1× 2) − 11 m expressed as: eterization isz the following: C(m ⇤ C (T 7 vector v can beualso CDefining Panchores L ↵ = ( , ) ⇧ R , C C eterizations. It consists on substituting the unmeasured A line can also be represented by the end points it.2 Applying For each point, the transformation and projection of a pinhole camer tor m and scalar ρ. 1 2 ⇤ = KR p ) ⇤ ) m /⇤ (17) m L = ⇧ R 11 n −n 0 L = = [x y m m m ρ] ∈ R . 0 R The complete homogeneous point paramT T 2 v 1 m i 0 i 1 1 HP L⇤2 ) p 0 for 0In 0this y from z expressed as: xt expression: 2 L 1m ⇤(m v ⇧ ⇥transformation anyx value .T⇧ PlüAHP ckerwhere coordinates camera C Cy section, line parameterizations covLare 2 Ci =(9) ρR27parameterization, 1= ·i[x u (T p(14) (18 HP L = m ⇤] point ⇧ (17) . ⌃by 0 iy in � linverse-distance = of K ·3some R ·1 ,(n −)for T × v) π ev1�β� �n0⇧anchored �R ↵1zequation conformed a 4-vector, which is=KR composed by 0 zthe 0 m x)m i = 2handles C The complete anchores homogeneous point paramDefining = ( ⇧ R , vector can be also In order for toAHP be exactly inverse-distance and The whole transformation and projection process ⇤ ⌅ expressed as: the homogeneous an homogeneous-point 2 as previously stated 3. ρ � � m degree of freedon by a Gaussian prior that T ↵ � � � � m eterization is the following: where n = a × b, n = ab − ba, n, v ∈ R , and In order for to be exactly and Defining = ( , ) ⇧ R , vector v can be also 2 ⇤2 ofAn homogeneous 2 1 next: 2 The description frame is performed as shown T eFigure mthe p 2 (6) 2D line is obtained by the cross For each point, transformation and projection C ·2 R lthe =plane, K ·isC2(n T ⇤homogeneous v) I (9) 2 are 0 n, and3D ered. ofis: projection to image frame, bi= Tp0(m + − (T .m 2: Pl¨ uto cker line geometrical representation. Image ⇤and n + n be parallel the image e e obtained KR − p ) ρ) ∈ P . (7) where K instrinsic projection Pl¨ u cker matrix vector m and scalar ⇥. Pl¨ u cker coordinates in terms of R , T, v expressed as: line is an 11-vector defined as follows:   1 1 2 T An 2D line is obtained by the cross product of two po eterization is the following: 0 For each point, the transformation and projection of 1 T C Comparing this result to what was obtained fo infinite uncertainty but that is still manageable my the 1 ⇤ In order for to be exactly inverse-distance and e be parallel to the image plane, e and e are obtained expressed as: � � 3 3 4 The and projection process for 2 two pointsHomogeneous-points An homogeneous 2D line is obtained byproduct the2)cross � 1�m � geometr 1T 2 ⌥ RK Anchored line: Ano the following Pl¨ u∈cker v =C 0. K istransformation instrinsic projection ucker matrix v�,constraint: = · transformation ,homogeneous (13) of lying on it,2:l11Pl¨ =uucker × uline and thus, The from anchred to back-projection CFigure pinhole camera is , as For previously stated, u a the ⇤2point, LE =n = ,vCnext (8) where n = × having b, ab − ba, n,⊂ vPl¨ R and extracted from =parameterization pwhere = ρwhole =athe [m m ρ] ∈ Pconvertion R AHP pm ⇥ with z each theis linear aredefined An homogeneous line obtained by the cross 0ny = =previously [p m m2it⇢2of ]T 2 be R seen1 2) lcoordinates, =0projection K ·1n C convertion lying on it,transformation l= u1 ⇥ L uPl¨ , ,cker giving = Eexpressions: · [?] β, (13)2D ⇧ ⌃point aanchred pinhole camera is as stated, AHP L and 1 ⇢ R [T] can that2Anchored the(10) produc n where K by is the instrinsic projection Pl¨ ucker matrix 2u e, 1the be parallel tocan the image e1 following and eand ρ EKF.x The from homogeneous point to with next expressions: nincluded. × e1 as:lying C C obtained ⇤ ⌅ × Rplane, 2 are −1 product of two points on it, l = u ⇤ u and thus, way of representing a line is by the endpoints TL extracted from [?] Euclidean coordinates be achieved the C Pl¨ u cker coordinates in terms of R T, n, and v is:   ⇤ m defined as: L = H · = · . ⇤ 1 2 T C 1 RK u For each point, the transformation and projection of m P L L = = , (8) 3 4 having the following Plüiscker constraint: n vP L= expressions: 0.p00defined Tv vC= E · L,HP = p = eproduct (15) AHP am pinhole is ,R as previously stated, each point, transformation projection of way aline: pinhole camera lying onbe it, lPl¨ =u1cker u1 the ⇤ u and ⇤projection ⌅mthus, 2(13) anchores homogeneous point paramnFor = Kpoint, ·two (11) × a and vector orthogonal to of the planeisπ each and projection represen ==Euclidean mpoints ⇥]camera ⇤ PC For ⇥ 2)m Homogeneous-points Another 12of 2 is dteframe Cly T R as: withspeaking, ztransformation coordinates achieved the following 3 K[m isxof instrinsic matrix C (9) define it. Departing from the anchored homogen Cthe next 0stated 0Anchored �n �the 3where C transformation −1 T =n ·isRvalue ·× (n − Tn× v) nK the normal v Tthe ⇥ and 1 Iab − ρGeometrically Ca u line: line in by two points �process �lfor ρC v a pinhole camera ,(17) previously where ⇧where ⇥ C= any of·cker nas)can = K ·as lby v =(8) EvC·P∈ , defined (13) in equation 3. ⌥vector For differentequation: landmark types such as points l =is KR ((m ) stated, (TaPl¨line ⇥sub-vector (⇢ ⇢n. )) . that ( m =K u. from b, = ba, R eession: following: uCi== KR (m ⇤previously m1(12) analogous the upcker the term � 1T 0 )(11) 2 define 2 mAlso, RK u where m= ,A to v1) =Pl¨ E ,. and way of representing is by1 mthe endpoints i , as(T i )stated, a �pinhole camera T⇥ m2to Depar rder to convert homogeneous to L Euclidean AHP vC0n, ∈(13) πTTPl¨ for any of β. v, Cvalue etransformation + e=2 p0from α 0 0 equation: � � C1−it. 1K·−1 1is 2K·previously point parameterization, an homogeneous-point T defined as: v 0 0 u = KR (m − (T − p ) ρ ) (17) . α 0 T l KR ((m × m ) − (T − p ) × (ρ m ρ m )) . u cker coordinates camera u i 0 i v C n = · l (11) 1 2 0 1 2 2 1line C i C C Line’s range and orientation expressed in v are not  uncertainty transformation and projection for the planen πisThe containing the line and the and An= homogeneous 2D line the crossjoining product of two points speaking, thewhole vector normal to C lines, has distict implications. For points, (ρ ρobtained isbyfor a the vector the twothe suppor C must Cbe ⇤following ⌅Tprocess a = [a a] and b =and b] can where ⇤v given as prior. � Geometrically �following having the Pl¨ ufor cker constraint: n = 0.. beC represented it. Departing homogeneous n2 homogeneous −n 0as 1m 2· − 2 m1 )from vCEuclidean · define e eisdefined (12) T ⇤ Cexactly By comparison to+ the obtained Pl¨ uanchored cker coordinates, prod m ⇤=origin, 1 111-vector 2result 2)Another point parameteri T 1 next: where ⇧ ⇥ any value of[b . vto In order convert from to 2 C �n� as follows: ates, the equation is applied: 2) Anchored Homogeneous-points line: In order for to be inverse-distance 0 α 0 0 α 0 T K = . K u v frame is performed as shown (18) u = KR (m (T p ) ⇤ p (17) p Pl¨ u cker line back projection is shown in figure 3. ⇤ ⌅ u u v 0 u 0 u v l = KR ((m ⇤ m ) (T p ) ⇤ (⇤ m ⇤ m )) . Pl¨ u cker coordinates trans C i 0 i 2 0 where � e = · �n � (14) T omogeneous point parameterization m − (T − p ) ρ) ∈ P . (7) Pl¨ u cker coordinates in terms of R , T, n, and v is: n n 0 i upoints 0 on(12) 1l = 2u 2parameterization, 1 of the line, therefore T p= + b. . all vthe (6) 1homogeneous vl = = β11((m ·22D e1 +�2line β2m ·2is eand related 11-vector to Pl¨ ucker C where Korigin, the instrinsic projection Pl¨ uTCcker matrix ⌃m measurable. 1⇥ 2 , giving ⌅⇥In be prior. for value �β�as toAn befollowing inverse-distance the plane containing the and the and there in distance and covers point an homogeneous-point linesub-vecto isdefine v·isgiven isuncertainty the director vector from aitp2⇤0(9) to Euclidean � 0=lπ=ρRK Corder �way �⇧ by cross ⌦ Kmust · Ru−α −asT dwhere = homogeneous 6-vector, known Pl¨ uexactly coordinates: −1 ⇥amline is athe vector orthogonal to the plane ⇡, analogous to the Pl¨ uan cker s 0lying 0it,T)homogeneous C is KR p (⇤ ⇤2 m . obtained where ⇧ for any . cker 2� C 1 ⇤ 22)obtained 1m 2 endpoints 1 )) vC, �n T Cρvalue eline bevv) parallel to the e11 CThe and representing is⇤ by the that coordinates, equation applied: −α v(14) αuthe αof nobtained � are �1(T =u m ,(n (8) ⇧image ⇥ C✏ forplane, any .·K −1 vu 0 αuwhere v 1v α1× α 2D is the cross −1 ⇧two R and eof mutually (18) framebywhat isp performed asthe shown nlines nC1of 0eare (m p )An ⇤+i0Also, )m m v u0until u 0defined ueα v = nC1�eKR + nis 1 , plane, 2of(11) 1, e 2on ⇥for ⇥e22�be v.(17) 0 as: ⇧ expressed ⌃ C0 i it, (T 2 ithe �v� parallel to0 theproduct image eC1u and obtained KPwing 22speaking, 11-vector defined as follows: n�handles = · the l nC vector ucoordinates KR (m (T p )line ⇤2(18) /⇤ (17) i = 2 p .are (6) points l vector = u= ⇤ thus, Comparing this was gives obtained for visual ray infinity. straight 1 be 2lying Geometrically n is normal to In order exactly inverse-distance and 0 i )camera 2result v isequations: the director fromwith a to b. The Euclidean n. (⇢ ⇢m that direct 0u Rare [T] n C vector i= When Pl¨ cker in 1p 2 and orthogonal distance from the line to the origin is given 1m 2m 1 ) is2 atovector C Cto C 0 0 the nextInfinite expressions: define it. Departing from the anchored Ttermhomogeneous K = . B. 3D line parameterizations ✏ e = · (14) ×uR � � ρ u ρ C 1 product of two points lying on it, l = u × u and thus, n + n =   where , ⇧ R and e , e , n are mutually C C orthogonal. ⇤ L = H · L = · . l = KR ((m ⇥ m ) (T p ) ⇥ (⇢ m ⇢ m )) . m homogeneous 2D isuof obtained the 0cross 2 For 1line 2 �Tparallel 1 be 2 seen 1 coordinates, 2 by 1vectors 2 1 2 that 1 the ⇥2order ⇥line P inverse-distance L CPl¨ hores point paramin two 1 which C homogeneous m CL and and with next expressions: where ⇤order must be given asto Pl¨ cker can product 2�· the Pl¨points uline cker back projection, n and v(11) In to exactly When u2nthe cker coordinates are in between the line,line therefore related to Pl¨ ucker sub-vector uncertainty degrees of freedom, correeK the image plane, e and eorigin, 0parameterization, R where βobtained βand ∈nPR eAn , nC1 to are mutually = ev + · and e32,expressed (12) × e⊂ B. 3D line parameterizations vCan the pparameterization. 5 6, e Cprior. Cβ the plane πgiven containing and the ρorthogonal frame, isvwhat only obtained by result was obtained for The projection and transformation process isβ1be 1× 2 are distance from the line to the origin is 1 ,Comparing 2Tcamera 1projection 2u α 0v 0for 1 1(9) 2R point homogeneous-point is an itthis 1this 0 In for to be exactly inverse-distance Figure 4 shows 11 l= ·be Rb, ·Cframe (n T × v) C v− by �n�/�v�. Thus, �v� is the inverse-depth, analogous Rv. m n + n When Pl¨ u cker coordinates are expressed in camera .wing: where n = a n = ab − ba, n, ∈ and L = = [n n n v v v ] ∈ P R , p = .O (2) v 0 u 0 u v orthogonal. ⇤ C L = ⇧ R = (15) P L be xobtained y orthogonal. z byx e are yeand z 1projection 2to product of two points lying on it, l = u ⇤ u and Comparing this result to what was obtained for 1= H AHP Lthus, 2 An m × m is a vector orthogonal to the plane π, In this section, some line parameterizations cov� By comparison to the result obtained for Pl¨ u cker coordinates, the product homogeneous 2D line is obtained by the cross frame, is only 1 2 L · L = 1 2 (5) e parallel the image plane, e are obtained 2 C n ⇤ e C   with the next expressions: m v p = .O (2) spond to a distance that should be covered up to infinity, are computed according to these expressions: An homogeneous 2D line is obtained by the cross given in the next expression: P L 1 1 2 1 vector 0ucker αline 0 ea 11-vector defined ascan follows: K = . are Pl¨ u cker coordinates, it can be seen that the product 1↵ PL e be parallel to the image plane, and obtained �n � T ρ u e1from v is the director to b. The Euclidean � � � � by �n�/�v�. Thus, �v� is the inverse-depth, analogous Defining = ( , ) ⇧ R , vector v be also 1 2 C where K is the instrinsic projection Pl¨ u cker matrix 1 2 0L   to ρ of homogeneous points. Pl¨ geometrical   In this section, some line parameterizations are covT frame, projection is only obtained by m The projection and frame transformation process is C e2 ered. =⇤ The � v to � The ust be having the following Pl¨ uCcker constraint: n =(15) 0. Landmark initialization whole transformation and projection process for ⇥coordinates, � given as prior. Pl¨ um cker can be seen that the product −1 analogous to to Pl¨ ucker Also, u  + Tρ m m isto a⇤u vector plane analogous to the Plüthe ckerterm subdescription of projection image frame, biproduct of two lying l⇥ ⇤ and ((m ⇤ m )a coordinates (T porthogonal )itC2.3. ⇤it, (⇤ m m )) .thus, ⇤α,efrom 1of 21 with the expressions: ρsub-vector C l�=as: L ∈ 2R11n.  thus, 1 Pl¨ 2points 0on 1= 2 1in −α un v⌅0T∈next α α 2orthogonal Cthe product two points lying on it, lthe = u u⇡, and 1 Cnext Kand uallofpoints. possible orientations. In the sections, point HP L 1=⇤ T T description m ⇤ isterms vector the plane ⇥, 0Cβ u1 vline e pto T 2and nvn with the next expressions: When are expressed camera where β ,expressed e2 , norigin areKR mutually 1 2� oint 21 m 2of next 0 m p orthogonal the to1(7) the given ρparameterization of = homogeneous u3D cker line geometrical B.Pl¨ line parameterizations n(m Defining = ,2also ⇧2Cthe R ,the v can be also 1p0 l(= KR mvector )(⇢plane − (T − p2is (ρ −consist ρ22 mgives ))on. two ered. The projection to image frame, bielinear = (15) where K projection uRuP cker matrix ⇤that 20)((m uis =the KR − (T ∈ .back-projection  Pl¨Defining uare cker coordinates R ,vector T,expression: and v1= is:K given in the as: 1 0a)1a)× 2 1the 2distance 1−C β= , in βuT2cker )∈ R , vector vn, can be C defined Cis C (β C 2instrinsic (ρ× m ρm vector joining support 0 )2 ρ)Pl¨ H = representation is shown in figure 2. The process of initialization landmark the direction detection m m is a orthogonal to ⇥, vector n. Also, term ⇢ m is a vector the 1⇤ −1 1 22 − 2m 1 )of transformation and included. (18) 1 2 1 2 m l · n (10) C n −n 0   ✏ e = · n (14) 2 uisand mrepresentation parameterizations for ULIThus, for 1 used C , line (8) to the uis cker sub-vector Also, the term 2 analogous 1projection orthogonal. ρ =linearRK The whole transformation a  n.  image, m1the Pl¨ uanalogous cker back projection figure 3. frame, isCPl¨ only obtained by ⇥ arencovered, ⇥is (18) transformation are included. expressed as: s: defined as:When by �n�/�v�. �v� thevector inverse-depth, l in =K · n⌅the (10) Tshown shown in�back-projection figure 2. vector 2⇤ is expressed as:�line � eIn = · l�n (14) of the line, therefore related Pl¨ usub-vector cker sub-vector he camera isand the director of Pl¨ cker coordinates are expressed camera a1in feature in to1ρtransformation 3D, inclusion into the m �ρm �uthe between points of011 line, therefore related to Plütocker v. speaking, the to 1normal analogous the Pl¨ u2cker sub-vector n.points the term ) the two C 2 + n ameterizations camera frame, is the director vector of the 2retroprojection T =is�KR ((m ⇤ m (T pAlso, )the ⇤ m . and � (⇤ � Cm 2cker � frame, For each point, and 2 1m 2(7) ⇤2the nC (5)InCinitialization ρ C C3 are ρC −1 then deepen this some parameterizations ⇤C�this )to a=tovector joining in3Geometrically the process itself. ρthe u KR (m (T ) ⇤) ⇧support P . (⇤ LTHP = are ∈v. R Plü)) coordinates inprojectio terms obtained 1projection 2 isAshown αdependence 0section, 12m2 22m1result Comparing what was for 1p0 Cpoints L � n�line n 0 cov1) isn Pl¨ uline cker line: in Poptical by two v Pl¨ uthe cker line back in figure 3. Cl = K 2 defined C complete anchores homogeneous paramframe, projection only obtained by Points represented asresult a 2-vector containing Cartesian coordinates in po to ρinverse of homogeneous points. Pl¨ u line geometrical ⇤ ⌅ n + n 2point 1cker ·0plane n The (10) C T (⇤ m ⇤ m ) is a vector joining the two support T T ray, and ⇥ has a linear dependence to the inverse   ray, and ρ has a linear to RK u + Tρ (a) (b) (c) T 1 2 1 2 2 1 ⇤ ⌅ Defining β = (β , β ) ∈ R , vector v can be also the π containing the line and the origin, and α 0 0 (18) Comparing this to what was obtained for 1) Pl¨ u cker line: A line in P defined by two points v = E · , (13) C pinhole camera is , as ⇤previously stated, 1 T2 C bi-· C T frame, eCT1 projection = Cb ✏ nucker (14) = coordinates, KR m p2.3. ⇤Landmark m ⇤a2For m )) .in Cv of las = ((m K ·Line’s ×(T v)seen πand (9) m of the· Rline, therefore related to(⇤Pl¨ u cker 1⇤ 2−) Trange 0 )that 2 1each point, the1not transformation l= KR ((m ⇤ mmodeled (T (⇤ m . projection Pl¨ it(n can be the product on, some line parameterizations are cov2 Therepresentation projection to For eachofpoi orientation expressed initialization nin n 0 lpoints 11 2 )sub-vector 0v) ⇤are 2 2 m1 ))and Pl¨ uiscker line back isand shown figure n e1n ⇥b]orientation 0 [b αcan 0description thea. following: K T=and = a] and = image [b can be represented T ered. space, are as apcan Gausian variable. 2C ⇥ 1 3. n10arange 0to ueterization C not 2 C b = isα2from in 2. of therefore related to Pl¨ ucker sub-vector Line’s expressed vCpoints are  be seen C the expressed as: 0as⇤[a Pl¨ uand cker coordinates, itcamera that the(18) product .n of the distance dein1defined from the center to the C= 2 C C a represents =d � [a a] ρ represented ushown ✏(15) eK distance defined from thevb]optical center the e1figure nC1 ×⇤ Figure 4 paramshows this parameterization. is thebetransformation director vector The Euclidean Cto �iption ρ2plane (18) or and 2 == = (14) E ·line, β,optical (13) aofpinhole as previously stated, m vector orthogonal v. of projection toA.image frame, bi+ n ✏�  b. eto = n (14) 3D point parameterizations given as prior. v = to E ·⇥, ,vector (13) C a pinhole camer linear and back-projection included. uisa ,landmark ⇥= ⇥2 m·2v.nis av measurable. Thethe process initialization of consist on the detection of 1 homogeneous 1· are 2ucker The complete anchores homogeneous point Comparing this result what was obtained for l = K · n (10) 6-vector, known as Pl¨ � −1 2 coordinates: n e (15) −α u α v α α ⇥ ⇥ measurable. 2 −α u α v α α m × m is a orthogonal to the plane π, v 0 u 0 u v C C C 2 2 u = ⇠ N {¯ u , U} homogeneous 6-vector, known as Pl¨ u cker coordinates: v 0 u 0 u v point. 1 2 KLine’s Cto the Pl¨ pC0 are l )= K) the · RTmap. · (n Cnot uvalue cker sub-vector Also, the term mation anduback-projection are included. +analogous n �n ln = K⇥·line n T−1 orthogonal distance from the originn(10) is given v2CC �⇧K⇥ any ofthe . following: ordinates. v KR where is for theeterization instrinsic projection Pl¨ un. cker matrix athe feature incan the image, retroprojection toTn. 3D, and (T inclusion into range orientation expressed in v 1where + nCC2to⇤the is Pl¨ u cker coordinates, it be seen that the For each point, transformation and projection ofproduct u = (m p ⇤ p = � �and 1 i 0 i 2 Comparing this result to what was obtained for n e analogous to the Pl¨ u cker sub-vector Also, the term Figure 4 shows this parameterization. i C C  3 Comparing this result to what was obtained for C 1 This section explains some point parameterizations. RK u LAHP = Amline (8)points C ,by two (⇤vectors mas: m is aFor joining thehomogeneous two support C 2(15) C Figure nFor Pl¨ ucamera cker line back projection, vectors n the and Tdefined 1)ucker Pl¨ cker line: in Pexpressed 2vC n 1 )vCany ρWhen coordinates, Tv 6⇤ Points are represented containing Cartesian in pixel eis= = ⇧ ⌃aa 2-vector 41vector shows parameterization. n When uline cker coordinates are inn camera Pl¨ udefined cker back projection, ∈ for value of β. by �n�/�v�. Thus, �v� the inverse-depth, analogous vline = Ex· Cβ, (13) T uPl¨ am pinhole is ,In previously stated, C is Cas 1itproduct ⇢ 0 )coordinates izations m ⇤ m aseen vector to plane measurable. 6in Cx n u. iseen = KR (mthe −⇥, (T −p ρ) (17) L v3. ⇧where P15 ⌅ ,πand ⇤ ⌅ Pl¨ cker coordinates ine: A line P3�uprior defined by two points 2this (ρ m − ρorthogonal )ucan isasof vector joining two support in figure Cy ⇥(a) =beare Pl¨ uRcker coordinates, itrelated can be the ithe P Lcamera z vn y⇤vezIn �m� 1] order 1for 2and 2m where v ⇧ ⇥ any value C Pl¨ uaccording cker coordinates, be that product ρ=C ⇤2ve1refer LP Linto = [n vx vare v ]expressed ∈Line P Rdescription , projection T5 ⊂back Tis=shown ρ[n The aspects included toebe the n be given as and x nrepresents y nzFigure yPl¨ points of the therefore to Pl¨ cker sub-vector (5) d and for to exactly inverse-distance ui = 3.in used for initialization. Plücker line geometrical representation; T ¯ u Ui 0 computed tothat these expressions: p C �line, � Line’s range orientation expressed in v are not Inb order imspace, and are modeled as an. Gausian variable. =line (15) ==is 0u vρbe Plüvcker line back projection is shown invector figure 3.to frame, projection isparametrizations only byPl¨ are according togeometrical these expressions: a of = zhomogeneous [a a] each and b and =nobtained [bC b]computed can as 2 represented −1 CT can Line’s range orientation expressed in are not related analogous the Pl¨ u cker sub-vector Also, the term where K is the instrinsic e = (15) u = ⇠ N {¯ u , U} = N , to ρ points. u cker points of the line, therefore to Pl¨ u cker sub-vector C d α 0 0 nd = [b b] represented as 2 An homogeneous 2D line is obtained by the cp m ⇤ m is a orthogonal to the plane ⇥, v frame, projection only obtained by 1 u + Tρ m ⇤ m is a vector orthogonal to the plane ⇥, 1 2 neThis v.Figure 2 = me line parameterizations are cov⌥ RK 1u , parameterization itself, camera projection, coordinate toto the plane, eAHP and=1e2 are obtained 3: Pl¨ uimage cker back-projection. Image extracted 1 0 0 In order for �β� to line be exactly and m the origin of to coordinates. u (8) C n 1 be parallel 1 inverse-distance m.v. L measurable. C Cas Pl¨ C ucker allows express the unbounded distance measurable. as a reference T (⇤ m ⇤ m ) is a vector joining the two support homogeneous 6-vector, known coordinates: For Pl¨ u cker line back projection, vectors n and v defined as: 0 α 0 u = ⇠ N {¯ u , U} -vector, known as Pl¨ u cker coordinates: K = (b) Plücker line back-projection; (c) anchored homogeneous-points line parametrization. where v ∈ π for any value of β. In order for to be exactly inverse-distance and 1 2 2 1 An homogeneous 2D line is obtained by the cross product of two points lying on it, l = u ⇤ u C u analogous to the Pl¨ u cker sub-vector n. Also, the term representation is shown in figure 2. C analogous to can Plü− cker n. Also, term coordinates u KR (m − (T ρi ) by a 4-vector (17) 1 2 and Cp0 ) sub-vector Lines be expressed thatthe represents of t projectionto to image frame,the bi- unbounded from [?] e be parallel to the image plane, e and e are obtained ithe with the next expressions: transformation, and where back-projection. v ρdistance must be given as prior. i = ρ 1 1 2 ⇤ s�of allows express Pl¨ u cker line back projection is shown in figure 3. ⇤ An homogen ofC a point along the optical ray to points, infinity, Figure 4 shows ⇥ l =2K · nC −α u1this αu 0 αline, points of the therefore related Pl¨ u sub-vector product two lying ondensity it, l = ufunction. of back-projection the landmark. ve 0 uparallel 0 v are computed according totothese be to epoints are obtained (⇤ ) 3. isαparameterization. avfrom vector joining the two 2figure C 1 × u⇤2 and Figure 4:cker Anchored homogeneous-points linethus, parame 1 1ofand 2 probability (⇤ ⇤2image m is4plane, anCsupport vector joining the two support end with aeto Gaussian and areIn 1m 2 in ⇤ 2m mogeneous point: order im- expressions: the expressions: C In homogeneous coordinates, (10) 1m 2 the 1 )also Figure shows this parameterization. For Plücker line projection, vectors n and vwith  ⇢ Tincluded. Plü�β� cker line projection is next shown ⇢   product v two p 5 ray 6from 0 Pl¨ ninfinity, Inback order for to exactly inverse-distance and For Pl¨ u cker line back projection, vectors and v T be back oint along the optical to of u cker line back projection is shown in figure 3. into this bounded interval in parameter space ⇥ ⇤ 5 6 v. = [n n n v v v ] ∈ P ⊂ R , C x y z x y z ¯ u u U 0 points of the line, therefore related to Pl¨ u cker sub-vector with the next expressions: terization. Image extracted from [?] cludes the ¯ 1 UK0= u1 related u =line =e[n vxtovthe vimage P ⌅ eR1 and , 2When points of the line, therefore toS} Pl¨ ucker An homogeneous 2D line sis the cross anchor addedbyastwoa points reference to P L3D x n(10) y nto z these yexpressions: z ] ⇧ plane, are computed = K · nLB. 3 anchor Pl¨ ucker coordinates are in camera 0 ⇠by N{¯ {¯ = N sub-vector are obtained C ⌅ expressed u==obtained ⇠ N us,,U} = N , , 0 U linebounded in Pis defined vparameterizations are computed according to points these expressions: (0, ⌃m⌃/demin ].v. isinitialization interval inl landmark. parameter space ρ according ∈1 be parallel ⇤ isC only where ⇤line be given as isprior. ¯(T 0p0 )0 ⇤ (⇤1 m2u ⇤2 m T 2u KR 2 lu1= ⇤ m2 ) 1 u complete Pl¨ ucker parameterization thel = product of must two it, × uT2 ((m and 1thus, v. 2.2.1. Plücker Line nates ofTand the point frame, The projection C Figure 1T time of the 4 showslying thison parameterization. prior represents with the next expressions: Line’s range and orientation expressed in vC are not v 0 u n nobtained 0 �by = [b b] can be represented as � 2 1 T l = KR ((m × m ) − (T − p ) × (ρ m − ρ m C Lines can be expressed by a 4-vector that represents coordinates of1 )) their In this section, some line parameterizations are covThe probability density function for infinite lines like Pl¨ u cker, pdf N. ¯l 1 2 0 1 2 2 �/d ]. scalar. C C following: ✏ n e1 transformation =Figure · nC (14) −nan 0⇥ parameterization. measurable. The frame of homogeneous point s coordinates. a min 6-vector the anchor 2shows 1 this fr,erse-depth known as Plüthat cker includes coordinates: T 4 ⇥ C of projection to image frame, biFigure 4 shows this parameterization. ⇤ ⌅ 2 2 2 (18) ered. The description end points, also with a Gaussian probability density function. in conformed by the homogeneous line representation, and the Gaussian T � = line �n � extracted (14) lu= KR ((m1 ⇤ C C � · Image Line’s range and orientation expressed in v are not Pl¨ cker coordinates  C3 ⇢ When INITIALIZATION 1 ucker 3:ePl¨ back-projection. B. 3D�next line�nparameterizations n�the III. by L ANDMARK is Cperformed to 2+ nC 0 coordinates are conformed by�TaFigure six-vector that represents a line in )nP defined two 1lC= 2CC 2 Cartesian coordinates of point 2 according ¯ 1 to u Uwhat 0 was obtained linear transformation back-projection 2(10) 1ufollows: C included. tribution as K+· Tnnequation: 1 ComparingC this result � are frame transformation of the anPlücker homogeneous point nl�1= KR For Pl¨ ucker line backand projection, vectors n v is onlyforobta from [?] m2 )�−✏ (Tdefined − sp= (ρ m= T ⇠ 2N−{¯ N. frame, , projection ·sρ, 2S} n (14) 0 × Comparing 1m 1 )) 2 e1 measurable. this result to what was obtained � ((m �1 × = C and C int: order to C vy van ∈ P5im⊂ R6 , Pl¨ u cker coordinates, it can be seen that the pro n −n 0 ⇥ ⇥ T T ¯ u u 0 U x ny nIn z vx z ] inverse-depth C section, some −1 anchor, and scalar. 2 2 n ⇤ e 2 2 2 1 In this line parameterizations are covPoints are stacked as a 2-vector containing Cartesia C T 3 1 Figure 4: Anchored lineproduct paramen K l are computed according these expressions: (18)ithomogeneous-points rmed according next equation: 1)[ a Pl¨3: uacker line: in[eb P defined two a Figure = btoline= ]TPl¨ Pl¨ um cker can beorthogonal seen that the =] : �by (14)n=C H uand cker A line back-projection. 1 b nC nC (15) × e1The med ∈ R7 . to the ¯l,plan =⇥ H ⇤coordinates, mImage a infinite topdf � Image �2 points �extracted �2 · �ne2� e = L y mas z ρ] a reference topoints 1 to+ 2 The probability density function for lines ucker, Na Gausia L , Comparing 1 2 is C=(15) Corientation 9vector T coordinates inbipixel space, andlike arePl¨ modeled asthe projection image frame, terization. extracted from [?] RvC Tdescription =P LCand nered. Line’s2 range expressed in v are not C Eβof m × m is a vector orthogonal to the plane π,the Pl¨ ubCcker line Image afrom =Figure [a a] 3:and = [b b]Cback-projection. can be represented asC2 extracted nC1 + n 1 2 [?] For Pl¨ u cker line back projection, vectors n and v C p = Hp = p , thisin Comparing result to what was analogous to line theforrepresentation, Plücker sub-vector n. Also, �back-projection �n ��0transformation C (3) conformed by the homogeneous and the Gaussian Pl¨ ucker coordin variable. ime of the landmark. linear and areobtained included. n lterm =disK· measurable. 1−1parameterization The complete Pl¨ ucker line is ⇤ thee(16) T 2:�Pl¨ 1 analogous " # � to the Pl¨ u cker sub-vector n. Also, the homogeneous 6-vector, known as Pl¨ u cker coordinates: Figure 2: Pl¨ u cker line geometrical representation. Image 7 from [?] RK l + T × REβ � � C Figure u cker line geometrical representation. Image Plücker coordinates, it can be line seen that according to Figure 4: Anchored homogeneous-points m2 the⇤2product m ) is a vector sup defined as(⇤paramefollows: =are [x0computed y0 z0 the mx anchor mFigure Rparameterizations. . expressions: e2 =tribution (15) joining n × e1 y mzRρ] m1two ⇤thesupport mtwo , 3. � � 2 is a C at includes following: T∈2these 1: Point Image extracted u joining the (ρ1 m1to −the ρtwo ) is1 aπ,vector Figure 4:1Pl¨ Anchored homogeneous-points line 2by 2 mplane 1points C[?] Pl¨ ucker line back projection is shown invector figure REβ e2 = from (15) extracted from C inn ogeneous point to m × m afrom orthogonal 1) uextracted cker line: A line Pv3 Cdefined T Figure back-projection. points ofparamethe line,u therefore related sub-v = ∼ N {¯ u ,to U}Plücker to terization. Image extracted from [?]=Plücker n line Cn For Pl¨ uline cker line back projection, and p= Hp = pCL, 3: (3) TImage 5isvectors 6n[?] Pl¨ ucker back projection is 2shown in figure 3. 6 analogous the �n �5 ⊂extracted v points of the line, therefore related to Pl¨ u cker sub-vector = [n n n v v v ] ∈ P R , [?] 0 1 from T T rdinates of the point terization. Image extracted from [?] L = = n n n v v v ∈ P ⊂ R , P L x y z x y z Figure 3: Pl¨ u cker line back-projection. Image extracted where super-index C indicates the frame to which III.theLterm ANDMARK INITIALIZATION y zImage xaccording ya = z to analogous to the Also, Line’s range and orientatio uucker representation. PL geometrical TheFigure complete Pl¨ cker line line parameterization is xthe [?] v 2: Pl¨ are computed expressions: by the following [a these a] and b Pl¨ =ucker [b b]sub-vector can v.bev.n.represented as9 (⇤1 m2 ⇤2 m1 ) v [?] from �matrix �homogeneous-points �ρ26-vector, � the (ρ mPl¨ m−1 )prior. is aknown vector joining the two support inverse-depth scalar. 4: Anchored line The complete Pl¨ ucker line parameterization the rom anchred homogeneous point following: to the point isisFigure refered, H frame measurable. 2− In homogeneous coordinates, where βand must be as a1line extracted from [?] usepecifies cker back projection iscoordinates: shown in figure 3. homogeneous asparamePlücker C 1provided Points are4stacked asthis a 2-vector containing Cartesian points of the line Pl¨ u cker line back projection is shown in figure 3. n K l Figure shows parameterization. Figure 4: Anchored homogeneous-points line paramepoints of the line, therefore related to Pl¨ u cker sub-vector terization. Image extracted from [?] following: to which the point is transformed. Figure 4 shows this parameterization. ⇥ L = H = H = es can be achieved by the following P L C coordinates in pixel space, and are modeled as a Gausian Plücker coordinates camera re super-index C indicates the point: frame to from which v. line vv. L ANDMARK 1) Euclidean The parameters of Euclidean Figure 3: line Plüan cker line back-projection. terization. ImageT extracted from [?] n Eβ INITIALIZATION For Plücker back proje Pl¨transformation ucker coordinates transformation from camera The complete Pl¨ ucker parameterization is the2 Image extractedIII. 5 6 � � (16) variable. T L = = [n n n v v v ] ⇧ P ⌅ R , L ANDMARK P LIII. y INITIALIZATION z is 3 ,complete 7 asconsist ⌥ab Cback-projection. frame shown next: −1 Figure 3:⌥ Pl¨ line back-projection. Image extracted point on its Cartesian coordinates. The projection of aparameterization point into nt is yrefered, matrix Hissepecifies frame The Plücker line is image the 3: ufrom cker line extracted vT the RK l +the misz performed ρ] and ∈R . frame � are � computed according following: where n =Figure a ×the b,Pl¨ v = [?] ba, n, vKu⌥cker ∈1Image with the Plücker constraint: nTxthis vframe 0.x y z3Cartesian x m performed as shown next: Points are stacked as aREβ 2-vector containing Figure 4tosho n⌥− l Rperformed Figure 4×shows parameterization. , = Figure 4: Anchored homogeneous-points line parameu from [?] transformation cker from camera following: � �LPPl¨ �Lu= with coordinates the following expression: H =H REβ from [?] 1= T� coordinates h the pointm is (6) transformed. 3 nC C Points are stacked as a 2-vector containing Cartesian u= ∼ N {¯ u, U} K l C in pixel space, and are modeled as a Gausian III. L ANDMARK INITIALIZATION E =RPl¨ pframe =cker [x yn z]performed ⇤geometrical R � � � � v n isas EP 2: RInLterms [T] v terization. Image extracted from [?] of geometry, the vector normal to the plane π containing the line and the origin and v L = H = H = is shown next: Figure 4: Anchored homogeneous-points line parameC Figure u line representation. Image P L .⌥ C E Anchored line parameC coordinates in pixelFigure space,4: and are modeled as a Gausian (16) III. homogeneous-points L ANDMARK INITIALIZATION p = pL0P+ . · LP L = (6) × C · variable. nRE L =H C⌥R C [T]×vR1 ⌥ 2 0L Rto· L l +K T�u1⇤ The Pl¨ cker line isare the Points stacked as aas⌥2-vector containing Cartesian vframe terization. [?]Image omogeneous to projection a point the frame is ρofpoint ⌥agiven Lextracted = image H =[?] ·1by . ,parameterization terization. extracted from [?] is given n complete lthe The projection camera isRK � � where �β�must from �(16) � Image �extracted P variable. In homogeneous coordinates, provided aPfrom prior. Tbe 2 from C= L isinto the director from toThe b. The Euclidean orthogonal distance the line to the origin C= KR (m LPPvector H 0 = H −1space, Cn u T⇥) ⇤ . (4) R L = following: v Points are stacked as a 2-vector containing Cartesian C u mation process is RK l + T ⇤ RE ⌥ coordinates in pixel and are modeled as a Gausian K l R [T] R complete Pl¨ u cker line parameterization is the REE vPlücker Cline following equation: The complete parameterization evedwith by the med the following following expression: × LP Lis u == ⌅ N {¯ u, U} in pixel space, and are modeled as a Gausian H nC C . = H Lprocess ·1 L ·the P L⌥= Hfor (16), = v= u coordinates variable. P L =inverse-distance, Eβ RE v The whole transformation projection following: byprocess knkand /kisfollowing: v k. Hence, k v k is the analogous to ρ of homogeneous points. Plücker line 0 R u ⌅ N {¯ u , U} v nd frame transformation RK l + T ⇤ RE ⌥ III.v Lvariable. ANDMARK INITIALIZATION � � T (16) The whole and process KR T) ⇤is: P2 projection (1) , forExpressing an−1In homogeneous in the camera 3 Plücker coordinates in terms ofu R= ,transformation T, n,(pand RK l+ Tu ×= REβ u point � RE homogeneous where mustv be provided as ⌥a prior. ⌥ camera pression: L ANDMARK� INITIALIZATION ⌅uNare {¯ u ,Km U}III. C III. L ANDMARK INITIALIZATION , iscoordinates, geometrical representation is in Figure Cvand Pl¨ coordinates transformation 13a. u ) ∈ P2 .u = KR (7)T (mPl¨ cker ∈ coordinates inThe terms of be R ,provided T, shown n, and is:from Points stacked as a 2-vector containing Cartesian frame, the projected image point = , and v whole transformation projection process for REβ n K l In homogeneous coordinates, must as a prior. −2uTρ) P2u.cker where (4) ⌥H ⌥= 1 u= ∼ N {¯ u, U} ⌥ ⌥ L = H = C P L C v is performed as shown next: coordinates in pixel space, andCartesian area modeled as aand Gausian 1and where, is not measurable. Back-projection then: Points are stacked as 2-vector containing Figure 3: Pl¨ ucker line back-projection. Image extracted n Khomogeneous lline E Figure 3: Pl¨ ucker back-projection. Image extracted Points are stacked as ais 2-vector containing P . frame (7) vKC of cker coordinates terms R⇥ , inverse-transformation T, n, and v is: nC as in Expressions for transformation of Plücker coordinates from toCartesian l . (m − (T − p0 ) ρ) ∈ (6) In coordinates, where Plü must be provided a prior. L = H = H = T LP L = H (9) = H ⌥ P L � 1 �= v C from (16) in pixel 3 variable. [?] coordinates in pixel space, and are modeled as a Gausian C l = K · R · (n − T × v) π E from [?] � � coordinates space, and are modeled as a Gausian In homogeneous coordinates, where β C⇤ must be provided as a prior. E v ous point paramRK l + T RE ⌥ T ⌥ n (16) (16) Figure 4: Anchored homogeneous-points line parameressing an homogeneous point inlframe theK camera the camera are as−⌥shown next: Ruv) [T] Figure 4: Anchored homogeneous-points line par variable. , 3 = (n TRK × (9) 1Image variable. C·Figure 3: Pl¨ cker line R back-projection. extracted u 1 nchores homogeneous point param= H·C·R l +TT×⇤ RE · RERK terization. Image extracted from [?] ⌥ u= ⌅⌥ N {¯ u, U} C 3 .l + T ⇤ RE Pfrom L =[?] terization. Image extracted from [?] , #Figure the projected imageispoint is uLP=L Km , Land 0 R , v l = K · R · (n − T × v) π (9) 3: Pl¨ u cker line back-projection. Image extracted v sformation process u " " # u RE  llowing: RE where K is the instrinsic projection Plücker matrix 3 homogeneous-points complete ucker parameterization u= ⌅line N {¯ u, U} uline =parameterization u, is U} Figure 4: parameThe The complete PlüPl¨ cker line isthe the measurable. C ⌅ N {¯ from [?] v vAnchored Tot  Back-projection  where Kisisthen: R [T]× R terization. following: Innhomogeneous coordinates, where must beuprojection provided asprocess a prior. � defined � as: Image extracted from [?] C the instrinsic projection Pl¨ cker matrix following: The whole transformation and for Figure 4: Ancho −1 p0 T L = H · L = · (9)INITIALIZATION PL instrinsic u , 2 (8)�−1 defined as: where must bePL provided as0 a prior. where Kbe isprovided the projection ucker matrix � mustin The complete cker line parameterization is the In R homogeneous coordinates, where a,uprior. III. III. L ANDMARK terization. Image vC In homogeneous coordinates, ρ) ∈ P . RK (7) terms of asRPl¨ T, n, and vPl¨ is: L ANDMARK INITIALIZATION 0(8) coordinates 0 0) m u αv , Plücker C = following: � as: � 0defined � −1 " ⌥ � Cn3C � The ⌥� complete Pl¨ u cker line parameterization is the " # # Points are stacked as a 2-vector containing Cartesian 0 αu K =ρC . K l ρ 1 αv 0 � 0 Points are stacked as a 2-vector containing Carte � LPT K l = =Hn T INITIALIZATION Cfollowing: coordinates in pixel space, and are modeled as a Gausian L3P LR=L H −R = α 0−1 0 −αv u0 αu vK0 = αu αv 0 n [=T=H]H× 3III.EEβ L ANDMARK Cv geneous point paramcoordinates in pixel space, and are modeled as a Gau α v T C 0 v �. � � � � � u (16) (10) Cv) l = KK· R=L·PL (n − −1 ⌥ RKT Points are ·stacked as a 2-vector containing 0=T ×nH αu · L PL 0K−1=l . (9) (16) variable. variable. Cartesian � � III. L 1 l + T × REβ v −αv u0in camera αu v0 Lα be given as Pl¨ prior. =v H =H = 0 R , uα RK l + T ⇤ RE PL ⌥ C When u cker coordinates are expressed u coordinates in pixel space, and are modeled as a Gausian REβ −αv u0 vαu v0 αu αEβ ⌥ , ⌥ C v  ∼ u, U} u N {¯ 1u= � � RE (16) v variable. Points are sta frame, projection is only obtained by n u = ⌅ N {¯ u , U} K l −1 T lu +cker T × REβ v When where PlückerKcoordinates inRKcamera LP L = �H � C = H = isWhen the instrinsic projection Pl¨ matrix coordinates in pi −1 Plüare ckerexpressed coordinates are expressed in, camera E u v erizations In homogeneous coordinates, where β must be provided as a prior. REβ RK u , (8)frame,defined = is as ∼ N {¯ u, U}In homogeneous coordinates, The transformation of R, T,asna and follows: projection only projection obtainedand by as: is (16) variable. must be provided prior.u v v⌥ frame, is projection only obtainedprocess by wherein terms 1 C

RK l + T ⇤ RE rizations are covρ line parameterizations ome � � l = are K · covnC (10) β must αwhere 0 be provided 0 In homogeneous coordinates, as a prior. RE v frame, bi3 nimage of projection to image frame, biT 3 =. K · R · (n (10) − T × v) αu 0 Cl(10) K =l = K ·0nC tion included. nor.andareback-projection are included. l = K · n Line’s range and orientation expressed in vC−α arev unot αu v0 αu αv 3 0 where must be provided as a prior. 3 ned by two Ameasurable. line in Ppoints defined by two points C matrix K is Cdefined as: where the intrinsic projection Plücker T Line’s When range expressed v are not in camera Plüorientation cker coordinates areinexpressed Line’s range and orientation expressed in v are not e =represented [b b] canasbe represented as and C C For Plüas cker projection, vectors nis and frame, projection onlyv obtained by measurable. measurable. tor, known Plüline ckerback coordinates: cker coordinates:   meterizations cov- to these expressions: are computedare according α 0 0 C C C Cvectors v n Tto image frame, biFor Pl¨ u cker line back projection, n and v T line back projection, vectors n and  v 5 z vx 6 n] x ∈ nyPn vy, vz ] ∈ P5For ⊂ Pl¨ R6ucker ,  ⊂ Rincluded. are computed according ojection are K =expressions: l= K · nC to these 0 (10) αu 0  are computed according to these expressions: 

− α v u0

defined by two points n be represented2 as Plücker coordinates:

y

Line’s2 range and orientation expressed in vC are not measurable.

α u v0

,

In homogene 3

αu αv

When Plücker coordinates are expressed in the camera frame, projection is obtained by:

For Plücker line back projection, vectors nC and vC are computed according to these expressions: l = K · nC

T

v z ] ∈ P5 ⊂ R 6 , 2

The range and orientation of the line are included in vC and are not measurable.

u

(11)

(12)

Sensors 2016, 16, 489

7 of 25

For Plücker line back projection, vectors nC and vC are obtained with these expressions: n C = K −1 · l

vC = β 1 · e1 + β 2 · e2

n o where β 1 , β 2 ∈ R and e1 , e2 , nC are mutually orthogonal.

Defining β = ( β 1 , β 2 ) ∈ R2 , vector vC can also be expressed as: vC = E · β,

where vC ∈ π C for any value of β. Plücker line back projection is shown in Figure 3b. The complete Plücker line parametrization for back projection and transformation is given in the following equation:

L PL = H

"

nC vC

#

=H

"

K −1 l Eβ

#

=

"

RK −1 l + T × REβ REβ

#

(13)

where β must be provided as a prior. 2.2.2. Anchored Homogeneous Points Line A line can also be represented by the end points defining it. With the application of the anchored homogeneous point parametrization, shown in Figure 3c, an anchored homogeneous-points line is an eleven-vector defined as follows:

L AHPL = [p0 m1 ρ1 m2 ρ2 ]T ∈ R11 For each point, the transformation and projection of a pinhole camera is as previously stated in Equation (4). A homogeneous 2D line is obtained by the cross product of two points lying on it, l = u1 × u2 , giving: l = KRT ((m1 × m2 ) − (T − p0 ) × (ρ1 m2 − ρ2 m1 ))

(14)

In comparison to the result obtained for Plücker coordinates, the product m1 × m2 is a vector orthogonal to the plane π, analogous to the Plücker sub-vector n. Furthermore, the term (ρ1 m2 − ρ2 m1 ) is a vector that gives the direction between the points of the line, therefore related to Plücker sub-vector v. The complete anchored homogeneous point line parametrization for back projection and transformation is the following:

L AHPL



   =  

C where ρC 1 and ρ2 must be given as priors.

p0 m1 ρ1 m2 ρ2





      =    

T RK−1 u1 ρC 1 RK−1 u2 ρC 2

      

(15)

2.3. Landmark Initialization The process of the initialization of a landmark consists of the detection of a feature in the image, retro-projection to 3D and inclusion into the map. There are three important concepts that are involved in the landmark initialization: the 3D landmark x itself, the 2D measurement z of the landmark in

Sensors 2016, 16, 489

8 of 25

the image and the unmeasured degree of freedom π. All of these are modeled as Gaussian variables, whose notation is N µ, σ2 . Thus, the cited concepts are expressed as x ∼ N {x¯ , P}, z ∼ N {z¯ , R} ¯ Π}, respectively. The 3D landmarks x considered for this study are points and lines, and π ∼ N {π, already described in Sections 2.1 and 2.2. The parametrizations for landmark 2D measurements z and unmeasured degrees of freedom π, as well as the description of the initialization algorithm are covered in the following sections. 2.3.1. Landmark 2D Measurements in the Image Points are represented as a two-vector containing Cartesian coordinates in pixel space, leading to the following: " # u ¯ U} , u= ∼ N {u, v where U is the covariance matrix of the position of the point. In homogeneous coordinates, u=

"

u 1

#

¯ U} = N ∼ N {u,

("

u¯ 1

# " ,

U 0

0 0

#)

Lines can be expressed by a four-vector that represents the coordinates of their end-points, also with a Gaussian probability density function. s=

"

u1 u2

#

∼ N {s¯ , S} = N

("

u¯ 1 u¯ 2

# " ,

U 0

0 U

#)

¯ L , is composed of the The probability density function for infinite lines like Plücker, N l, homogeneous line representation and the covariance matrix defined as follows: ¯l = u¯ × u¯ , and 1 2

¯ 2 ]× U [u¯ 2 ]T L = [u¯ 1 ]× U [u¯ 1 ]T × + [u × 2.3.2. Unmeasured Degrees of Freedom The uncertainty in 3D points and lines coming from projection is represented by inverse-distance variables ρC and βC , which are modeled as Gaussian variables. The origin of each of these priors must be inside the 2σ of their probability density functions. For points and end-point-based lines, the minimum distance must match the upper 2σ boundary, hence: ρ − nσρ = 0,

ρ + 2σρ = 1/dmin

0≤n

How you get there from here: interaction of visual landmarks and path integration in human navigation.

A method for projecting retinal landmarks into visual space.

Statistical shape analysis: From landmarks to diffeomorphisms.

Interactive visual analysis of heterogeneous cohort-study data.

A Visual Interface for Querying Heterogeneous Phylogenetic Databases.

Correction: Competing Distractors Facilitate Visual Search in Heterogeneous Displays.

Landmarks in three dimensions: reconstruction from cephalograms versus direct observation.

Historical landmarks of autophagy research.

Some landmarks in Australian immunology.

Landmarks in simple pleural effusions.

Visual landmarks sharpen grid cell metric and confer context specificity to neurons of the medial entorhinal cortex.

Objects and landmarks: hippocampal place cells respond differently to manipulations of visual cues depending on size, perspective, and experience.

Anatomic Landmarks of the Distal Radioulnar Joint.

Landmarks for the insertion of oesophageal endoprosthesis.

Visual inspection after acetic acid (VIA) is highly heterogeneous in primary cervical screening in Amazonian Peru.

Neurocognitive development of memory for landmarks.

Landmarks in Allergy during the 19th Century.

Further landmarks in teaching primary care.

Editorial: LANDMARKS IN DEVELOPMENTAL BIOLOGY AND EVOLUTION.

Positional relationships of abdominal aorta landmarks for angiography: observations from the intravascular space.

Clinical landmarks for intervertebral spaces in children.

Recording projection of nasal landmarks in rhinoplasty.

Methods for determining hip and lumbosacral joint centers in a seated position from external anatomical landmarks.

Visual snow--persistent positive visual phenomenon distinct from migraine aura.