IROS2019国际学术会议论文集 1593

Precise Correntropy based 3D Object Modelling With Geometrical Traffi c Prior Di Wang1 2 Jianru Xue1 Wei Zhan2 Yinghan Jin3 Nanning Zheng1 and Masayoshi Tomizuka2 Abstract Robust 3D perception using LiDAR is of prime importance for robotics and its fundamental core lies in precise object modelling resisting to noise and outliers In this paper a precise 3D object modelling algorithm is designed especially for the intelligent vehicles The proposed algorithm is advantageous by leveraging the crucial traffi c geometrical prior of road surface profi le and both the noise and outliers are elegantly handled by robust correntropy based metric More specifi cally the road surface correction RSC method transforms each indi vidual LiDAR measurement from its locally planar road surface to a globally ideal plane This procedure essentially guarantees the reduction of vehicle s motion from arbitrary 3D motion to physically feasible 2D motion To deal with the noise and outliers a correntropy based multi frame matching CorrMM algorithm is proposed which has a robust objective function with respect to point to plane residual error An effi cient solver inspired by M estimator and retraction technique on Lie group is developed which elegantly converts the optimization of highly non linear objective function into a simple quadratic programming QP problem Extensive experimental results validate that the proposed algorithm attains more crisper 3D object models than several state of the art algorithms on a challenging real traffi c dataset I INTRODUCTION In recent decades the LiDAR based 3D perception is widely recognized as the indispensable module for mobile robotics especially for the intelligent vehicles 1 2 One of the fundamental aspect of 3D perception is precise ob ject modelling which is extensively utilized as the core component within motion estimation robust localization and semantic segmentation Roughly speaking there exist two mainstream paradigms in 3D object modelling namely minimizing transformation error and minimizing registration error The main idea of minimizing transformation error is to properly amortize global tranformation error into each frame s local coordinate In 3 motion average MA is extensively employed in order to guarantee the loop closure constraints by averaging the errors in Lie group Torsello et al 4 represented 3D motion by dual quaternion and a diffusion is performed on the view adjacency graph The main limitation of aforemen tioned algorithms is that they discard the useful point cloud pertaining to each frame and the global transformation error This work was partially supported by the National Natural Science Foun dation of China project under Grants 61751308 61773311 and U1713217 1 Authors are with Visual Cognitive Computing and Intelligent Vehicle VCC IV Lab Xi an Jiaotong University Xi an 710049 P R China 2Authors are with the Department of Mechanical Engineering University of California Berkeley CA 94720 USA 3 Author is with Zhejiang University Hangzhou 310058 P R China Corresponding author s email jrxue a b c d Fig 1 The proposed CorrMM is robust to noise and outliers and produces satisfying 3D object model in real traffi c scenarios cannot be essentially decreased by distributing the error into each frame s local coordinate To overcome the aforementioned problems Li et al 5 enhanced the original MA by incorporating a robust pair wise registration into the inner loop Bonarrigo et al 6 proposed an objective function which summed up the Eu clidean distance between all possible point cloud frame pairs but this least squared LS formula is problematic when outlier ratio is large With a coarse initial guess Zhu et al 7 improved the registration accuracy by performing a coarse to fi ne mechanism iteratively However the high computational burden is unavoidable considering each point cloud is compared and optimized with all the other point clouds during coarse to fi ne process Instead of computing registration error from point clouds directly Evangelidis et al 8 re formulated the 3D object modelling as the probability density estimation problem and a Gaussian mixture model GMM is proposed to effi ciently approximate the generative process of each individual point cloud The drawback lies in the high computational costs and slow convergence rate when employing expectation maximization EM algorithm Most of the aforementioned algorithms are designed and validated on dense point clouds which guarantees the re liable computation of overlapping region and the rejection of outliers However the point clouds in intelligent vehicles 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 IEEE2608 are from low spatial resolution multi beam LiDARs which are notoriously sparse and easily affected by un predictable participants in real traffi c scenes In 9 Moosmann et al proposed a moving object map ping procedure which incrementally built up the 3D object model via point to plane iterative closest points ICP algo rithm However the object model may be inaccurate since the 3D rotation estimated by ICP can be physically infeasible for vehicles Zeng et al 10 proposed to approximate the object s surface with a GMM model and enforced the rotation part to be SO 2 during optimization The limitation lies in the effi cacy of 2D rotation is only guaranteed on planar road surface which can be easily violated in urban traffi c scenarios Held el at 1 proposed a probabilistic latent surface model and maximized the likelihood by employing an effi cient annealed dynamic histograms However the rigid transformation in 1 is assumed to be translation only and the iteratively optimizing mechanism is not fully tested and validated Compared with dense point cloud the sparse point cloud in intelligent vehicles endures a large portion of noise and outliers Several robust objective functions are proposed for the pair wise regitration Bergstrom et al 11 recommended to employ robust M estimator which essentially imposed an adaptive weight on naive point to point residual errors and iteratively reweighted least squares IRLS is proposed to ef fi ciently attain the rotation and translation Du et al 12 pro posed a correntropy based pair wise registration algorithm which is proven to be effective even when large portion of outliers exists It is worth noting that the resemblence between correntropy and M estimators is pointed out in 13 In this paper we proposed a precise 3D object modelling algorithm especially designed for intelligent vehicles The proposed algorithm takes advantage of traffi c geometrical prior and generates satisfying 3D object model as illustrated in Fig 1 The main contributions of this paper are two folds 1 the road surface correction RSC method transforms each individual LiDAR measurement from its locally planar road surface to a global ideal plane which enforces its rotation components being SO 2 2 To robustly deal with noise and outliers the correntropy based objective function is extended with multi frame point clouds and point to plane distance is employed instead of point to point distance to mitigate the negative effects caused by the sparsity of the input point clouds Considering the resemblence between correntropy and M estimators the objective function of the proposed algorithm is fi rstly approximated by IRLS from M estimators 11 and then the variables pertaining to SO 2 manifolds are locally projected into R with retraction tech nique from the Lie group 14 Consequently the original correntropy based objective function is elegantly reformu lated as a simple quadratic programming QP problem Extensive experimental results prove that the proposed al gorithm gains better accuracy in real traffi c scenarios than several state of the art 3D object modelling algorithms Fig 2 RSC is performed for a track within an intersection Notice that the road profi le changes rapidly in this scenario The red points are raw LiDAR measurements accumulated for several seconds of the track and they are attached with the complex road surface The black points are corrected by RSC which are lying in an ideal plane II PROBLEMFORMULATION In order to investigate the behaviours of vehicles in dense traffi c the static ego vehicle equipped with LiDAR sensor is parked within an intersection to record the traffi c fl ow Notice that in this scenario precise motion estimation is of prime importance and the real time requirement can be relaxed A Mathematical Notation The static occupancy map of the intersection is pre built i e M pm i nm i Nm i 1 where pm i nm i R3denote a 3D point and its corresponding normal vector The initial track for each individual vehicle is formulated by applying some pre process as indicated in subsection IV A Each individual track is represented by a collection of 3D point clouds namely Wa Pa i NWa i 1 where a denotes the track identifi er and Pa i pak nak NPa i k 1 denotes the ithpoint cloud or LiDAR measurement Since the proposed algorithm deals with each individual track separately the track identifi er a is dropped with a little abuse of notation i e W Pi NW i 1 denotes the track and Pi pk nk Ni k 1 denotes the LiDAR measurement To be more specifi c building the precise 3D object model for the track W is equivalent to fi nd out the optimal rigid transformations for all the LiDAR measurements R R1R2 RNW SO 3 NW T t1t2 tNW R3 NW 1 where Ri SO 3 and ti R3denote the rotation and translation for ithLiDAR measurement respectively III PROPOSEDALGORITHM A Road Surface Correction Intuitively the road surface constraint is benefi cial since the degree of freedom in rotation matrix can be reduced However an unitary planar road surface assumption 1 10 is too simplistic in real traffi c scenarios The main idea of the proposed road surface correction RSC method is the locally supporting road surface for each individual LiDAR measurement is relatively planar even though the global shape of the road surface can be 2609 quite complex Fortunately the corresponding normal vectors within this locally supporting road surface can be directly attained from occupancy map M The novelty of RSC lies in projecting the various supporting road surfaces into a global ideal plane imaging that the whole road surface is fi rstly stretched non rigidly to an ideal plane with its z axis aligned with the static ego vehicle s z axis then for each individual LiDAR measurement its locally supporting road surface is subsequently tilted to the ideal plane The tracked vehicle is lifted from the road surface to ideal plane as indicated in Fig 2 By introducing the ideal plane the motion of the vehicle is reduced from arbitrary 3D motion SO 3 R3to physically feasible 2D motion SO 2 R3 For each LiDAR measurement Pi pk nk Ni k 1 its lo cally supporting road surface can be found by performing the radius search between the centroid of Piand the map M If the mean normal vector of the supporting road surface is nrs and the normal vector of the ideal plane is npl 001 T the rotation can be calculated by rotating nrstowards npl with the followed rotation matrix tan 1 nrs npl 2 n rs Tnpl u nrs npl nrs npl 2 R exp b uc 2 where b c R3 so 3 denotes a skew operator which maps a 3D vector to Lie algebra so 3 and exp so 3 SO 3 denotes a mapping from Lie algebra to Lie Group B Correntropy based Multi Frame Matching In this sub section the correntropy based multi frame matching CorrMM algorithm is proposed and its objective function is effi ciently approximated and simplifi ed as a quadratic programming QP formula To recap the purpose of CorrMM is to fi nd out the optimal rigid transformations R T for track W Pi NW i 1 with LiDAR measurement denoted as Pi pk nk Ni k 1 For clarity p and q will be utilized as the notations of 3D point from two different LiDAR measurements 1 Objective Function The correntropy based objective function fusing multi frame LiDAR measurements or point clouds is formulated as J R T P i j C Ni P k 1 G rijk 3 where G r exp r2 2 2 denotes a Gaussian kernel and C denotes the correspondence pair indicating whether the two LiDAR measurements Piand Pjare draw from the same object part or they have enough spatial overlapping rijkdenotes the residual error and point to plane distance is employed to mitigate the negative effect caused by the sparsity of input LiDAR measurements rijk Ripk ti Rjqc k tj r2 ijk rT ijknc k n T c k rijk 4 where pkand qc k are points in ithand jthLiDAR measurements nc k denotes the normal vector in jthLiDAR measurement Ri ti and Rj tj are respective transfor mations in Eq 1 c k denotes the correspondence index computed by nearest neighbor search NNS namely c k argmin l kRipk ti Rjql tjk2 2 5 The transformations in Eq 5 are considered to be fi xed as the last iteration s R T Intuitively if all the point clouds are perfectly matched the residual error rijkwill become 0 and the objective function will attain its global minimum However the ob jective function in Eq 3 is highly non linear and hard to be optimized Since correntropy is closely similar with Welsch estimator 13 the solving techniques of M estimators 11 can be directly applied Thus the original objective function is reformulated as the followed iteratively reweighted least squares IRLS formula J R T P i j C Ni P k 1 ijkr2 ijk 6 where ijk G rijk denotes the scalar weight computed by last iteration s R T The objective function in Eq 6 is still hard to be optimized since each rotation component of R should be guaranteed to lie on SO 3 manifold during the optimization process 2 Retraction based Solver Lie group with its corre sponding Lie algebra is widely utilized in robotics 14 which smoothly maps Euclidean space R3into the manifold SO 3 with the exponential operator exp so 3 SO 3 Due to the high computation involved in the exponential mapping the retraction technique 14 is employed in this paper Compared with the original exponential mapping the retraction technique pursues a trade off between mapping accuracy and computational effi ciency In this paper the retraction operator for 3D rotation and translation is formulated as R3D ra SO 3 R3 R6 SO 3 R3 R0 t0 expb Rc R0 t0 t 7 where t R T R6and t R R3 Notice that this retraction operator is different from 14 and we have found that this retraction operator will induce more simple structure when approximating the residual error vector rijk as illustrated in Eq 11 Since the rotation is reduced from SO 3 to SO 2 after RSC the retraction operator is subsequently reduced into R2D ra SO 2 R3 R4 SO 2 R3 R0 t0 expb Rc R0 t0 t 8 where t T R4 and R 00 Tis defi ned in order to be compatible with Eq 7 With the retraction operator in Eq 8 the optimization of the objective function in Eq 6 can be cast to the followed optimization problem in the Euclidean space min R T J R T min J R T 9 2610 where R4 NWis the small perturbation applied for each rotation and translation in R T According to the defi nition of rijkin Eq 4 the retraction operator in Eq 8 and the approximation exp b Rc I b Rc 10 when Ris small enough the approximated residual error vector can be reformulated as rijk exp b Ric Ripk ti ti exp Rj Rjqc k tj tj rijk Hik i Hjk j rijk Hijk 11 where i j R4correspond to the respective component in and Jacobian matrices are Hik I bRipkc R3 4 Hjk h I Rjqc k i R3 4 Hijk Hik Hjk R3 4NW 12 It should be noticed that the matrix bR vkc is re duced to R3when rotation is SO 2 i e bR vkc R vk 2 R vk 10 T With approximated residual error vector in Eq 11 the optimization in Eq 9 is trans formed into a QP problem min TA 2bT s t 1 4 0 13 where A X i j C X k 1 ijkHT ijknc k n T c k Hijk R4NW 4NW b X i j C X k 1 ijkHT ijknc k n T c k rijk R4NW 14 The square matrix A is rank defi cient since the rigid trans formation R T is free to move in the space Therefore the transformation of the fi rst LiDAR measurement is anchored and set to be the identity rotation and zero translation or equivalently the fi rst four elements in is set to be zeros as indicated in Eq 13 The solution to Eq 13 is attained by solving a linear equation opt sub Asub I 1bsub 15 where Asub A5 4NW 5 4NW bsub b5 4NW sub 5 4NWand is a constant regulation factor to ensure that each component of is numerically small enough thus to guarantee the approximation accuracy of Eq 10 The whole algorithm is summarized in Alg 1 3 Implementation Details Due to the fi rst transformation R1 t1 in R T is considered to be fi xed during whole optimization process its respective Jacobian matrix is set to be H1k 0 in line 5 of Alg 1 To obtain the initial transformation R0 T0 a spanning tree is established by consecutively matching two point clouds Pi 1and Pi To Algorithm1 Correntropy based Multi Frame Matching CorrMM Input Track W Pi NW i 1 correspondence pair C initial value R0 T0 Gaussian kernel size regulation factor error tolerance maximum iterations tmax Output R T 1 for t 1 1 tmaxdo 2 Set A b to zeros 3 for i j C do 4 for k 1 1 Nido 5 Compute Hik Hjk Hijkvia Eq 12 6 Update A b via Eq 14 7 end for 8 end for 9 Compute optimal perturbation opt sub via Eq 15 10 if opt sub 2 then 11 break the loop 12 end if 13 Update R T via Eq 8 14 Update 0 95 15 end for 16 return R T a b Fig 3 The dataset is from a complex intersection In Fig 3 a the blue points are 3D occupancy map M and the red points are 2D curb points In Fig 3 b the red points are potential obstacle points after spatial difference between the input point cloud and M and the boxes are estimated on the curb map fi ltered obstacle points compute the correspondence pair C a ratio score indicating the percent of overlapping between two point clouds is computed for each potential pair and only pair pertaining to high ratio score can be incorporated into C Notice that C is fi xed during optimization for effi ciency consideration and it is computed based on R0 T0 As indicated in line 14 of Alg 1 The kernel size is slowly annealed to guara