IROS2019国际学术会议论文集 0898

Line based Absolute and Relative Camera Pose Estimation in Structured Environments Haoang Li Ji Zhao Jean Charles Bazin Wen Chen Kai Chen and Yun Hui Liu Abstract 3D lines in structured environments encode partic ular regularity like parallelism and orthogonality We leverage this structural regularity to estimate the absolute and relative camera poses We decouple the rotation and translation and propose a novel rotation estimation method We decompose the absolute and relative rotations and reformulate the problem as computing the rotation from the Manhattan frame to the camera frame To compute this rotation we propose an accurate and effi cient two step method We fi rst estimate its two degrees of freedom DOF by two image lines and then estimate its third DOF by another image line For these lines we assume their associated 3D lines are mutually orthogonal or two 3D lines are parallel to each other and orthogonal to the third Thanks to our two step DOF estimation our absolute and relative pose estimation methods are accurate and effi cient Moreover our relative pose estimation method relies on weaker assumptions or less correspondences than existing approaches We also propose a novel strategy to reject outliers and identify dominant directions of the scene We integrate it into our pose estimation methods and show that it is more robust than RANSAC Experiments on synthetic and real world datasets demonstrated that our methods outperform state of the art approaches I INTRODUCTION Structured environments exhibit particular regularity like parallelism and orthogonality These structures are com monly abstracted by one of the following models Manhattan world 1 holds for the scenes with three mutually orthogonal dominant directions These directions defi ne the Manhattan frame MF Atlanta world 2 is composed of several directions orthogonal to a common vertical axis Mixture of MFs 3 consists of multiple independent MFs and represents the most general layout These models have been applied to various fi elds to improve the accuracy and or effi ciency such as SLAM 4 5 and scene understanding 6 In this paper for any of the above models we aim at leveraging its orthogonality and or parallelism to estimate the camera pose The camera pose can be classifi ed into two main cate gories i e absolute pose and relative pose 7 The absolute pose consists of the rotation and translation aligning the camera frame to the world frame and can be estimated by 3D to 2D correspondences The relative pose represents the relative orientation and position between two cameras and This work is supported in part by the Natural Science Foundation of China under Grant U1613218 in part by the Hong Kong ITC under Grant ITS 448 16FP in part by the VC Fund 4930745 of the CUHK T Stone Robotics Institute and in part by the National Research Foundation of Korea under Grant NRF 2017R1C1B5077030 H Li W Chen and Y H Liu are with the T Stone Robotics In stitute and Department of Mechanical and Automation Engineering The Chinese University of Hong Kong Hong Kong China J Zhao is with TuSimple Beijing China J C Bazin is with KAIST Daejeon South Korea K Chen is with Wuhan University Wuhan China Email yhliu hali mae cuhk edu hk can be estimated by 2D to 2D correspondences Numerous pose estimation methods exploit point correspondences 7 8 However in man made environments with low textures points are insuffi cient 5 To solve this problem several methods have been proposed to use lines For example 3D lines inherently encode the structural regularity like parallelism and orthogonality and thus have been exploited by several pose estimation methods 9 14 However these line based methods rely on highly nonlinear constraints and or less compact rotation parametrization They either fail to guarantee accuracy and effi ciency simultaneously 9 11 or require relatively strong assumptions 12 14 In con trast our methods achieve high accuracy and effi ciency and require weaker assumptions and thus have high generality We decouple the rotation and translation and propose an accurate and effi cient method to estimate the rotation We decompose the absolute and relative rotations and reformu late the problem as computing the rotation from MF to the camera frame To compute this rotation directly estimating its three degrees of freedom DOF together leads to relative low effi ciency 10 15 In contrast we propose a novel method to estimate its two DOF fi rst followed by its third DOF Specifi cally we fi rst effi ciently compute two DOF by two image lines whose associated 3D lines are aligned to two MF axes We exploit the orthogonality to simplify the two DOF estimation as solving a linear system We thus reduce the rotation space as an one dimensional parameter space which accelerates the rotation estimation Then we compute the third DOF by another image line whose associated 3D line is aligned to any MF axis Thanks to our two step DOF computation our absolute and relative pose estimation methods are accurate and effi cient as will be shown in the experiments Moreover our relative pose estimation method relies on weaker assumptions or less correspondences than existing methods 12 13 After computing the rotations we estimate the translations By decoupling the rotation and translation our relative pose estimation method is applicable to the case of pure rotation 16 In addition we propose a novel strategy to effi ciently reject outliers and identify MFs for the general case with redundant correspondences We integrate it with our pose estimation methods which outperforms the RANSAC based methods 12 15 Overall we solve the line based camera pose estimation problem by leveraging the structural regularity The main contributions of our paper are summarized as follows We propose an effi cient and accurate method to compute the rotation from MF to the camera frame We compute its two DOF fi rst followed by its third DOF 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE6914 We decompose the absolute and relative rotations and solve them by the above two step DOF estimation Our methods have high accuracy effi ciency and generality We propose a novel strategy to reject outliers and identify MFs We integrate it into our pose estimation methods and show that it is more robust than RANSAC Experiments on synthetic and real world datasets have shown that our methods outperform state of the art approaches II RELATEDWORKS In the following we review related works on the line based absolute and relative camera pose estimation respectively Absolute pose estimation The line based absolute pose estimation methods can be classifi ed into two main categories in terms of whether the structural constraints are used 9 11 or not 15 17 19 For the non structural constraint based methods Liu et al 17 defi ned a cost function w r t the reprojection error of lines They minimized this cost function iteratively but their method is prone to converging to a local optimum without appropriate initialization In contrast most non iterative methods 15 18 19 provide more reliable solutions For example Mirzaei and Roumelio tis 15 defi ned a multivariate polynomial system and solved it by the Gr obner basis 20 Zhang et al 18 defi ned a univariate polynomial and solved it by the eigenvalue method 7 However they are sensitive to noise due to the lack of effective constraints like the structural constraints Some works leverage the structural regularity for the absolute pose estimation 9 11 Cagliot 9 proposed a method based on coplanar 3D lines Kuang et al 10 designed an algorithm for the case that 3D lines inter sect at a common point These methods showed that the structural confi gurations simplify the model parametrization and or enforce effective constraints However the features they use are diffi cult to detect in practice Xu et al 11 investigated the parallelism and orthogonality of 3D lines but the accuracy of their method is relatively unsatisfactory Our method is non iterative and leverages the orthogonality and or parallelism avoiding the local minimum and also achieving high accuracy and effi ciency Relative pose estimation The line based relative pose estimation requires at least three views given general 2D to 2D line correspondences whose associated 3D lines have arbitrary directions 14 Traditional trifocal tensor 7 re quires at least 13 line correspondences across three images and thus is relatively unpractical To estimate the relative pose between two images several approaches exploit ad ditional constraints of the matched image lines 21 22 or 3D environment 12 14 Some works assume that the matched image lines are projected from two overlapped 3D line segments 21 or their midpoints are weak correspon dences 22 However these assumptions may not be satisfi ed due to partial occlusion or incomplete detection of lines Several relative pose estimation approaches leverage the structural constraints 12 14 Elqursh and Elgammal 12 proposed a method applicable to Atlanta world and mixture of MFs They assumed a confi guration composed of three 3D lines whose two lines are parallel to each other and orthogonal to the third However their method fails to handle three mutually orthogonal 3D lines Sala un et al 13 proposed an orthogonality free approach assuming two pairs of parallel 3D lines However its integration into RANSAC has low effi ciency since it requires sampling correspondence quadruplets to test hypotheses not just generate hypotheses Zhao et al 14 proposed to leverage the ray point ray structure defi ned by two coplanar 3D lines While it achieves high accuracy its effi ciency is unsatisfactory In contrast our method is applicable to three 3D lines that are either mutually orthogonal or two 3D lines that are parallel to each other and orthogonal to the third Moreover our outlier rejection and MF identifi cation strategy is more effi cient and robust than RANSAC as will be shown in the experiments III PROBLEMFORMULATION In this section we fi rst formulate the absolute and relative pose estimation in structured environments Then we intro duce our decomposition of the absolute and relative rotations A Formulation of Camera Pose Estimation a Absolute pose The absolute pose consists of the rotation R and the translation t aligning the camera frame C to the world frame W As shown in Fig 1 for the minimal case of estimating R and t we use three 3D to 2D line correspondences Lk lk 3 k 1 We assume that the 3D lines L1and L2 are mutually orthogonal and defi ne the MF M the 3D line L3is aligned to any axis of M b Relative pose The relative pose is composed of the rotation Ri jand the translation ti jbetween the camera frames Ciand Cj As shown in Fig 2 for the minimal case of estimating Ri j we use three 2D to 2D line correspondences lk l0 k 3 k 1 These image lines are associated with the 3D lines Lk 3 k 1 of which L1and L2are mutually orthogonal and defi ne the MF M and L3is aligned to any axis of M In addition Fig 2 shows that for the minimal case of estimating ti j we use two 2D to 2D virtual point correspondences xk x0 k 2 k 1 that are formed by the intersections of the matched image line pairs k 0 k 2 k 1 These image line pairs are associated with the 3D line pairs Lk 2 k 1 Follow ing state of the art line based methods 12 13 we assume that two 3D lines of the line pair Lkare coplanar B Decomposition of Absolute and Relative Rotations a Absolute rotation As shown in Fig 1 we decom pose the absolute rotation R as R RM CRW Mwhere RW Maligns the world frame W to the MF M and RM Caligns M to the camera frame C We compute the rotation RW Mas follows For the 3D lines L1and L2 their directions vW 1 and vW 2 in W are known Without loss of generality we associate L1and L2with the x and y axes of M respectively Accordingly their directions in M are vM 1 1 0 0 and vM 2 0 1 0 Based on the constraint vM 1 vM 2 RW M vW 1 vW 2 we obtain RW M vW 1 vW 2 vW 1 vW 2 6915 R t 1 L 2 L Image plane CameraCamera 2 l 1 l R R 3 l 3 p 3 n x y z 3 v 3 L Fig 1 We leverage three 3D to 2D line correspondences Lk lk 3 k 1 to estimate the absolute rotation R and translation t The coordinates of 3D lines in the world frame W are known b Relative rotation As shown in Fig 2 we decom pose the relative rotation Ri jas Ri j RM CjR M Ci and compute RM Cjand RM Ciindependently Note that the computation of RM Ciis identical to RM Cj For writing simplifi cation we denote both RM Ciand RM Cj by RM Chereinafter Therefore we reformulate both absolute and relative pose estimation as computing the rotation RM Cfrom MF to the camera frame We propose a novel method to compute RM C We fi rst estimate its two DOF and parametrize it by a single parameter in Section IV Then we estimate its third DOF in Section V IV PARAMETRIZATION OFROTATION FROM MANHATTANFRAME TOCAMERAFRAME In this section we fi rst use two image lines to parametrize the directions of two mutually orthogonal 3D lines aligned to two MF axes Based on these line directions we parametrize the rotation from MF to the camera frame A 3D Line Direction Parametrization Let L1and L2denote two mutually orthogonal 3D lines aligned to two MF axes and their directions v1and v2in the camera frame are unknown The image lines l1and l2 correspond to L1and L2 respectively The camera center and lk defi ne the projection plane k k 1 2 and its normal nkis computed by lkand the known intrinsic matrix We begin with considering the 3D line direction v1 Similar to our previous work 23 we defi ne the direction d that is perpendicular to the plane normal n1 We set d as a unit basis of the null space of n1 By rotating d around the plane normal n1by the unknown but sought angle we align it to the unit 3D line direction v1as v1 R n1 d where R represents the angle axis representation 7 Therefore we parametrize v1by the angle Each element of v1 satisfi es the form v1 i 1 i i 1 2 3 where 1 i A1 i B1 i is known and sin cos Then we introduce the parametrization of the 3D line direction v2 We leverage two constraint 1 v2is orthogonal to the projection plane normal n2 i e n 2v2 0 2 v2 is orthogonal to the 3D line direction v1 i e v 1v2 0 We substitute the above v1 into the second constraint and combine the above two constraints as a linear system We solve v2up to scale and thus v2is parametrized by the angle Each element of v2 satisfi es the form v2 i 2 i i 1 2 3 where 2 i A2 i B2 i is known and is defi ned above i ji j Rt 1 L 2 L i th camera 1 L i R i j R j 3 n x y z 3 v 3 L j th camera 3 n 3 v i i j j Image planes 2 l 3 l 1 l 2 l 1 l 3 l 1 i 2 i 1 i 2 2 1 1 1 x 2 x 2 x 2 i 1 x Fig 2 We leverage three 2D to 2D line correspondences lk l0 k 3 k 1 to estimate the relative rotation Ri j Note that we only observe lk l0 k but not their associated 3D line Lk We denote the 3D direction orthogonal to both v1and v2by v and obtain it by v v1 v2 Accordingly v is also parametrized by the angle Specifi cally each element of v satisfi es the form v i i i 1 2 3 where i Ci Di Ei is known and sin2 sin cos cos2 The norms of v2and v meet the constraint kv2k kv k p where is known We normalize v2and v by v2 v2 kv2k and v v kv k respectively B Rotation Parametrization Given the above unit 3D directions v1 v2 v in the camera frame we now defi ne the rotation RM Cfrom the MF M to the camera frame C We associate the 3D directions v1 v2and v with the x y and z axes of M respectively The coordinates of these directions in MF thus are vM 1 1 0 0 vM 2 0 1 0 and vM 0 0 1 Therefore we have RM C v1 v2 v i e RM C 1 1 1 2 1 3 2 1 2 2 2 3 1 2 3 1 1 Eq 1 shows that RM C is parametrized by the angle Note that v1 v2 v encode three DOF of the rotation RM C We effi ciently estimate two DOF of RM Cby the image lines l1and l2based on the linear constraints n 1d 0 and n 2v2 0 cf Section IV A Accordingly we parametrize the rotation RM Cby only one parameter that encodes the third DOF Compared with traditional rotation parametrization methods such as Euler angles 17 and quaternion 19 our RM C in Eq 1 is w r t only one parameter The one dimensional parameter space improves the effi ciency of the follow up rotation estimation V COMPUTATION OFROTATIONPARAMETER AND TRANSLATION In this section we fi rst estimate the third DOF i e the sin gle parameter of our rotation from MF to the camera frame Then we compute the absolute and relative translations A Rotation Parameter Computation We leverage the image line l3to compute the parameter of our RM C in Eq 1 as follows We denote the m th column of RM C by RM C m m 1 2 3 that represents the m th axis of MF Figs 1 and 2 show that the 3D line L3 which is associated with the image line l3 6916 is aligned to an MF axis Accordingly the unit direction of L3is v3 RM C m Based on the constraint that v3 is orthogonal to the known projection plane normal n3 computed by the image line l3 i e n 3v3 0 we obtain n 3 RM C m 0 2 By substituting Eq 1 into Eq 2 we obtain three different types of constraints w r t the parameter as 0 m 1 L3kL1 L2 1 0 0 m 2 L1 L2kL3 1 0 0 m 3 L1 L2 L3 3 where m 1 2 3 represent the cases that the 3D line L3is aligned to the x y and z axis of MF respectively and are composed of sin and cos cf Section IV A and are known which are computed by the known 1 i 3 i 1 2 i 3i 1 and i 3 i 1 in Eq 1 respectively Each constraint in Eq 3 is w r t sin and cos Most existing methods solve this type of constraints in a general way i e they transform it as a nonlinear univariate polynomial 11 or polynomial system 15 and solve it by eigenvalue 7 or Gr obner basis 20 respectively However they lead to relatively low effi ciency Note that our and denoted by hereinafter for writing simplifi cation are linear polynomials w r t sin and cos is a quadratic polynomial and does not contain the fi rst order item We propose to simplify them as sine functions to improve the effi ciency of solving Let A B and C D E We transform as A sin