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 AbstractRobust 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 vehicles 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 vehicles12. 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 frames 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, Xian Jiaotong University, Xian, 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 authors 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 frames 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 objects 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- estimators11, and then the variables pertaining to SO(2) manifolds are locally projected into R with retraction tech- nique from the Lie group14. 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 = PiNW i=1 denotes the track and Pi=pk,nkNi 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 = R1R2RNW SO(3)NW, T = t1t2tNW R3NW, (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 110 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-vehicles 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,nkNi 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= 001T, the rotation can be calculated by rotating nrstowards npl with the followed rotation matrix = tan1 ? ?nrs npl? 2,(n rs)Tnpl?, u = nrs npl ? ?nrs npl? 2, R = exp ?buc ? , (2) where bc: 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 = PiNW i=1, with LiDAR measurement denoted as Pi=pk,nkNi 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 22 ? 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 iterations (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 estimator13, the solving techniques of M-estimators11 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 iterations (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 robotics14, 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 technique14 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 RcR0,t0+ t ? , (7) where = tRT 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 RcR0,t0+ t ? , (8) where = tT R4, and R= 00Tis 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 R4NWis 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 + bRc(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+ Hiki Hjkj = rijk+ Hijk, (11) where i,j R4correspond to the respective component in , and Jacobian matrices are Hik= ?I bRipkc? R34, Hjk= h I?Rjqc(k)? i R34, Hijk= HikHjk R34NW. (12) It should be noticed that the matrix bRvkcis re- duced to R3when rotation is SO(2), i.e. bRvkc= (Rvk)2(Rvk)10T. 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 R4NW4NW, 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 Pi1and Pi. To Algorithm1 Correntropy-based Multi-Frame Matching (CorrMM) Input: Track W = PiNW 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