Visual-Inertial Odometry with Point and Line Features IROS2019国际学术会议论文集 1809

Visual Inertial Odometry with Point and Line Features Yulin Yang Patrick Geneva Kevin Eckenhoff and Guoquan Huang Abstract In this paper we present a tightly coupled monoc ular visual inertial navigation system VINS using points and lines with degenerate motion analysis for 3D line triangulation Based on line segment measurements from images we propose two sliding window based 3D line triangulation algorithms and compare their performance Analysis of the proposed algorithms reveals 3 degenerate camera motions that cause tri angulation failures Both geometrical interpretation and Monte Carlo simulations are provided to verify these degenerate mo tions which prevent triangulation In addition commonly used line representations are compared through a monocular visual SLAM Monte Carlo simulation Finally real world experiments are conducted to validate the implementation of the proposed VINS system using the closest point line representation I INTRODUCTION ANDRELATEDWORK Over the past decade visual inertial navigation systems VINS have seen a great increase in popularity due to their ability to provide accurate localization solutions while utilizing only low cost inertial measurement units IMUs and cameras The affordability size and light weight nature of these sensors make them ideal for deployments in a wide range of applications such as unmanned aerial vehicles UAVs 1 and mobile devices 2 When fusing camera and IMU data a key question is how to best to utilize the rich amount of information available in the images In particular most VINS can be categorized by whether they use indirect or direct image processing tech niques Direct visual methods provide motion estimates by minimizing costs involving the raw pixel intensities captured by the camera 3 4 By contrast indirect systems typically extract geometric features from the pixels and track their motion across the image plane with the most common being points 5 7 and or lines 8 13 Point features correspond to 3D positions in the space that are detected as corners on the image plane Lines which are most commonly seen in human built environments are detected as straight edges in the image Quite a few navigation works have been devoted to utilizing both geometric features to achieve robust and accurate estimation In particular leveraging the multi state constraint Kalman fi lter MSCKF framework 5 Kottas et al 8 proposed the use of a unit quaternion with distance scalar to model a line This work was partially supported by the University of Delaware UD College of Engineering the ARL W911NF 19 2 0226 JWS 10 051 003 the DTRA HDTRA1 16 1 0039 and Google Daydream P Geneva was also partially supported by the Delaware Space Grant College and Fellow ship Program NASA Grant NNX15AI19H Y Yang K Eckenhoff and G Huang are with the Department of Mechanical Engineering University of Delaware Newark DE 19716 USA Email yuyang keck ghuang udel edu P Geneva is with the Department of Computer Information and Science University of Delaware Newark DE 19716 USA Email pgeneva udel edu and incorporate line features into visual inertial odometry VIO In addition they provided an observability analysis showing that VINS with a single line feature suffers from 5 unobservable directions Later on Kottas et al 9 and Guo et al 14 proposed the utilization of line constraints e g parallel lines and vertical lines to improve the esti mation accuracy in structured environments such as indoor or Manhattan world where all lines lay along the 3 major directions Yu et al 11 designed a VINS system with line features suitable for rolling shutter cameras Utilizing the Pl ucker representation and orthonormal error states 15 for lines Heo et al 12 implemented an invariant MSCKF with point and line features using a Lie group representation of the state Zheng et al 13 proposed representing a line with two 3D endpoints and designed a stereo visual inertial navigation system leveraging both points and lines He et al 10 used IMU preintegration 16 to design a batch optimization based visual inertial SLAM with point and line features where line features are represented in Pl ucker coordinates 17 In many of these previous works different representa tions for line features were used without considering their impacts on estimator performance Leveraging our previous work 18 which offered a brief survey of line represen tations and proposed a closest point representation in this paper we conduct a performance evaluation for the commonly used line parameterizations In addition we in vestigate and identify the degenerate motions for line feature triangulation which cause poor line estimates and have large practical impacts for monocular VINS which might leverage line features The main contributions of this paper can be listed as follows We present a tightly coupled monocular visual inertial navigation system which leverages point and line fea tures and performs online spatial and temporal calibra tion We propose two sliding window based line feature tri angulation algorithms and compare their performances We identify 3 degenerate motions that cause these line triangulation methods to fail We investigate commonly used line representations and numerically compare their performances in a line fea ture based monocular visual inertial SLAM scenario Real world experiments are performed to validate the designed system with the CP line representation and the performances are shown to be improved than the system using points only II PROBLEMFORMULATION In order to properly contextualize our line analysis we fi rst formulate the standard visual inertial odometry problem 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 IEEE2447 when using both point and line features We defi ne the state vector for visual inertial odometry as x h x I x calib tdx c i 1 where xIdenotes the IMU state xcalibdenotes the rigid transformation between the IMU and camera tdrepresents the time offset and xcrepresents the cloned IMU states At time step k the state vector can be written as xIk hI k G q Ikb g Gv Ik Ikb a Gp Ik i 2 where Ik G q denotes JPL quaternion 19 representing the rotation from global frame G to IMU frame I at time step k GvI k and GpI k represents the IMU velocity and position in the global frame at time step k respectively Ikbg and Ikba represents the gyroscope and accelerometer biases We defi ne the error states of the IMU state as x Ik h Ik G Ik b g G v Ik Ik b a G p Ik i 3 where the error x is defi ned as the difference between the true state x and estimated state x that is x x x However for the JPL quaternion the error state is defi ned as where we have q q q h 1 2 1 i q 4 with denotes the quaternion multiplication 19 In addition to this IMU navigation state we also estimate the spatial calibration xcalib between the IMU and camera In particular our state vector contains the rotation from the IMU frame to the camera frame C I q as well as the translation from camera to IMU CpI Explicitly we have xcalib h C I q Cp I i 5 Moreover due to the nature of electronic hardware e g asynchronous clocks data transmission delays and electronic triggering delays the timestamps reported by each of the sensors will differ from the true time that the measure ments were recorded In this work we treat the IMU clock as the true time and estimate the offset of the aiding sensor relative to this base clock 20 21 We model the time offset tdas a constant value td tC tI 6 where tCis the time recorded on the sensor measurements and tIis the corresponding true IMU time If there are m cloned IMU poses at the time step k corresponding to the poses of the IMU at the true imaging times then xckcan be written as xck hI k 1 G q Gp Ik 1 Ik m G q Gp Ik m i 7 A System Dynamic Model The local linear acceleration a and angular velocity mea surements are modeled with additive noises and biases am a IGRGg ba na 8 m bg ng 9 where ngand naare the continuous time Gaussian noises that contaminate the IMU readings The dynamic model for this system is given by 19 I G q t 1 2 I t I G q t G p I t GvI t G v I t Ga t bg t nwg ba t nwa t t d 0 x calib t 06 1 x c t 06m 1 10 where nwgand nwadenote the zero mean Gaussian noises driving the IMU gyroscope and accelerometer biases Gg denotes gravity b c represents the skew matrix b c 0 and continuous time variable x t denotes the state value at time t After linearization the error state dynamic equation are x t FI t 015 6m 7 0 6m 7 150 6m 7 x t GI t 0 6m 7 12 n t F t x t G t n t 11 where FI t and GI t are the continuous time IMU error state and noise Jacobians matrices respectively and n t n g n wg n a n wa represents the system noises modeled as a zero mean white Gaussian process with autocorrelation E n t n t Q t To propagate the covariance Pk kat time step k the state transition matrix k 1 k from time tkto tk 1can be computed by solving t k F t t k with identity initial conditions Thus the discrete time noise covariance and the propagated covariance can be written as Qk Z tk 1 tk k 1 G QG k 1 d 12 Pk 1 k k 1 k Pk k k 1 k Qk 13 B Point Measurement Model As the camera moves through an environment point feature measurements can be extracted and tracked between images These camera measurements are described by zp Cxp nf x y z h x z y z i 14 where Cxp represents the 3D position of the point feature expressed in the camera frame According to our time offset defi nition 6 the feature Cxp in the sensor frame with reported time stamp t corresponds to the time t tdin the IMU base clock Hence we have Cxp C IRIGR t td Gxp GpI t td CpI 15 where I GR t td and GpI t td represent the IMU pose at time t td which will be denoted as time step k for simplicity in the ensuing derivations C Line Measurement Model Similar to 15 we adopt a simple projective line mea surement model which describes the distance of two line endpoints xs usvs1 and xe ueve1 to the projected line segment in the image zl x sl l2 1 l22 x el l2 1 l22 16 2448 where u v are the coordinates of the point on the image plane while l l1l2l3 is the 2D image line projected from the 3D line Cxl expressed in the camera frame D Multi State Constraint Kalman Filter MSCKF Each feature measurement whether it be a point 14 or line measurement 16 can be written generically as z h x Gxf nz 17 where Gxf represents the feature either a point Gxp or line Gxl IfGxf is kept in the state vector the problem size and thus the computational burden will grow unbounded quickly preventing real time estimation One way to avoid this is to use a null space operation 22 to marginalize these features To perform this the MSCKF maintains a sliding window of stochastically cloned historical IMU poses corresponding to past imaging times in the state vector and accumulates the corresponding feature measurements collected over this window In our implementation we clone these poses at the true imaging times in order to estimate the time offset as in 20 By stacking the measurements corresponding to a single feature and using the current estimates of the IMU clones in the sliding window we can triangulate this feature to form its estimate G x f The feature measurement 17 can then be linearized as z Hx x Hf G x f nz 18 where Hxand Hfrepresent the Jacobians w r t the state x and feature respectively Decomposing Hfwith QR factorization gives the following Hf Q e Qn R 0 QeR 19 Note that Q n is the left null space of Hf i e Q nHf 0 Multiplying 18 to the left by Q n yields a new measurement model independent of the feature error z 0 H0 x x n0z 20 As this new measurement relates only to quantities already contained in the state vector the standard EKF update can be performed Utilizing this null space operation for every feature tracked over the sliding window keeps the problem size and thus the computational cost bounded III LINEREPRESENTATION ANDJACOBIANS A Line Representation In order to incorporate line measurements into the estima tor we need to fi nd an appropriate representation for these features In our previous work 18 we summarized several line error representations including orthonormal quaternion and closest point shown in Table I and Figure 1a Note that qlis a unit quaternion and ql q l ql Given the 3D positions of two points pf1and pf2 expressed in the same frame corresponding to the same line xl we can obtain its Pl ucker coordinate Model 1 in Table I as 15 23 n l vl bp f1cpf2 pf2 pf1 21 TABLE I Line representation and corresponding error states Model LineError states 1 Orthonormalnl vl l l 2 Quaternion dl qlwith R ql ne ve bnecve l dl 3 Closest Point CP pl dl qlpl pl pl where nlrepresents the normal direction of the plane con structed by the two points and the origin while vlrepresents the line direction The distance from the origin to the line can be computed as dl knlk kvlk A minimal orthonormal error state land l is introduced by 15 for line feature based structure from motion SfM Based on Model 1 we can conclude the basic geometric elements of a line including a unit normal direction ne nl knlk a unit line direction ve vl kvlk and the distance scalar dl see Fig 1a With these geometric elements Kottas et al 8 proposed the use of a unit quaternion qland a distance scalar dlto represent a line Model 2 where the quaternion describes the line direction R ql n e vebnecve ql 1 2 l 1 ql 22 where lrepresents the error state of the line quaternion while R ql refers to the rotation matrix associated with ql The 4D minimal error states of the line include the quater nion error angle and the distance scalar error l dl More importantly if we multiply the unit quaternion ql with the distance scalar dl we obtain a 4D vector which can be considered as the closest point for a line in the 4D vector space i e Model 3 pl dl ql dl h q l ql i pl pl 23 where plis the 4D error state for the closest point of a line In this paper we will compare the performance of these line representations within a SfM framework B Line Measurement Jacobians We use the CP representation to show how to compute the measurement Jacobians for line measurements 16 For the line projection in CP form we have the following equalities l K 03 CL CL h CdlCn e Cv e i 24 CL C IR bCpIcC IR 03 C IR I GR t td IGR t td bGpI t td c 03 I GR t td GL 25 Therefore the measurement Jacobians can be written as z G p I z l l C L C L I L I L G p I 26 z I z l l C L C L I L I L I 27 z xcalib z l l C L C L xcalib 28 z G p l z l l C L C L I L I L l dl l dl G p l 29 Please refer to Appendix I for detailed derivations 2449 a b c Fig 1 Basic geometric elements for a 3D line a Sliding window based line feature triangulation b Degenerate motion analysis for line feature triangulation c IV LINETRIANGULATION In order to utilize line features in the MSCKF an estimate of its 3D line parameters is needed to linearize the measure ment model 17 From the above section it is clear that we need the basic geometric elements e g ve veand dl for a line feature Image line detectors such as Line Segment Detector LSD 24 extract two arbitrary endpoints of the line segment in each image Consider line feature xl which has been detected and tracked over the sliding window yielding a collection of line segment endpoint pairs we propose two algorithms for line feature triangulation based on these stacked endpoint measurements A Algorithm A Denoting the endpoints for a line in the i th image in the sliding window as xsiand xei see Figure 1b we obtain the normal direction of the plane iformed by the line xland the i th camera center Cinei bxsicxei bxsicxei 30 Since line xlresides on every plane i we have the following constraint Cin ei C1 CiR z N C1ve1 0 31 Therefore C1ve1 can be found as the unit vector minimizing the error on this constraint which is given by the eigenvector corresponding to the smallest eigenvalue of N N The transformation of a line expressed in frame Cito a representation in frame C1is given by C1dlC1ne1 C1ve1 C 1 CiR bC1pCicC1 CiR 03 C1 CiR CidlCinei Civei 32 C1dlC1ne1 CidlC1 CiR Cinei bC1pCicC1 CiR Civei 33 C1dlb i C1ne1 b i bC1pCicC1 CiR Civei 34 where bi bC1ve1cC1 CiR Cinei is a unit vector perpendicular to C1 CiR Cinei Given all the measurements from i 2 m we build a linear system as C1dl b i C1ne1 b i bC1pCicC1 CiR Civei 35 By solving the above system we obtain C1dl After this step we have recovered all the required line parameters C1ne1 eq 30 C1ve1 eq 32 and C1dl eq 35 The triangulated 3D line feature can be transformed to other frames i e global frame G as needed Note that this is a generic algorithm and we have made no assumptions on the correspondences of the endpoints B Algorithm B One of the classical methods to triangulate line features is based on the two intersecting planes e g 1and i The dual Pl ucker matrix L can be computed as L 1 i i 1 bC1v i e1c C1d i l C1n i e1 C1d i l C1n i e1 0 36 where 1 C1n e1 0 and i C1n ei C1n eiC1pCi The line geometric elements C1d i l C1n i e1 and C1v i e1 are computed based on 1and i In this work we offer a generalization of this method for m measurements In particular we solve for the line parameters using C1ne1 m i 2 C1n i e1 m i 2 C1n i e1 37 C1ve1 m i 2 C1v i e1 m i 2 C1v i e1 38 C1dl m i 2 C1d i l m 1 39 After linear triangulation we perform nonlinear least squares to refi ne the line estimates noting again that we have made no assumption of the endpoint correspondences 2450 C Degenerate Motion Analysis for Triangulation When using a monocular camera the ability to perform line feature triangulation is heavily dependent on the sensor motion In particular we identify degenerate motions that cause the line feature parameters to become unobservable thereby causing triangulation failures see Fig 1c and Tab II Letting C denote the center of the camera frame and L is the line feature and formulate a plane as shown in Fig 1c If the monocular camera moves along the direction ve of L or toward L with direction bve