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 Abstract3D 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, .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 914. 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 911 or require relatively strong assumptions 1214. 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 1215. 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, 1719. 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 911. 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 1214. 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 1214. 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 Lk3 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 Lk2 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 = RMCRWMwhere RWMaligns the world frame W to the MF M, and RMCaligns M to the camera frame C. We compute the rotation RWMas 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,0and vM 2 = 0,1,0. Based on the constraint vM 1 ,vM 2 = RWMvW 1 ,vW 2 , we obtain RWM=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= RMCjR MCi, and compute RMCjand RMCiindependently. Note that the computation of RMCiis identical to RMCj. For writing simplifi cation, we denote both RMCiand RMCj by RMChereinafter. Therefore, we reformulate both absolute and relative pose estimation as computing the rotation RMCfrom MF to the camera frame. We propose a novel method to compute RMC . 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 angleaxis 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,iis 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= v1v2. 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,Eiis known and =sin2(),sin() cos(),cos2(). The norms of v2and vmeet the constraint kv2k = kvk = p where is known. We normalize v2and vby v2=v2/kv2k and v=v/kvk, respectively. B. Rotation Parametrization Given the above unit 3D directions v1, v2, v in the camera frame, we now defi ne the rotation RMCfrom the MF M to the camera frame C. We associate the 3D directions v1, v2and vwith 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,0and vM = 0,0,1. Therefore, we have RMC=v1, v2, v, i.e., RMC()= 1,1 1,2 1,3 2,1 2,2 2,3 1 2 3 1 # .(1) Eq. (1) shows that RMC() is parametrized by the angle . Note that v1, v2, v encode three DOF of the rotation RMC . We effi ciently estimate two DOF of RMCby the image lines l1and l2based on the linear constraints n 1d=0 and n2v2=0 (cf. Section IV-A). Accordingly, we parametrize the rotation RMCby only one parameter that encodes the third DOF. Compared with traditional rotation parametrization methods such as Euler angles 17 and quaternion 19, our RMC() 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 RMC() in Eq. (1) as follows. We denote the m- th column of RMC() by RMC()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=RMC()m. Based on the constraint that v3 is orthogonal to the known projection plane normal n3 (computed by the imag