2 research outputs found
Robust and Fast 3D Scan Alignment using Mutual Information
This paper presents a mutual information (MI) based algorithm for the
estimation of full 6-degree-of-freedom (DOF) rigid body transformation between
two overlapping point clouds. We first divide the scene into a 3D voxel grid
and define simple to compute features for each voxel in the scan. The two scans
that need to be aligned are considered as a collection of these features and
the MI between these voxelized features is maximized to obtain the correct
alignment of scans. We have implemented our method with various simple point
cloud features (such as number of points in voxel, variance of z-height in
voxel) and compared the performance of the proposed method with existing
point-to-point and point-to- distribution registration methods. We show that
our approach has an efficient and fast parallel implementation on GPU, and
evaluate the robustness and speed of the proposed algorithm on two real-world
datasets which have variety of dynamic scenes from different environments
μ£Όνκ³ λ° μ§λ μμ±μ μν 3μ°¨μ νλ₯ μ μ κ·λΆν¬λ³νμ μ ν© λ°©λ²
νμλ
Όλ¬Έ (λ°μ¬)-- μμΈλνκ΅ λνμ : 곡과λν μ κΈ°Β·μ»΄ν¨ν°κ³΅νλΆ, 2019. 2. μ΄λ²ν¬.λ‘λ΄μ 거리μΌμλ₯Ό μ΄μ©νμ¬ μμΉν νκ²½μ κ³΅κ° μ 보λ₯Ό μ κ΅°(point set) ννλ‘ μμ§ν μ μλλ°, μ΄λ κ² μμ§ν μ 보λ₯Ό νκ²½μ 볡μμ μ΄μ©ν μ μλ€.
λν, λ‘λ΄μ μ κ΅°κ³Ό λͺ¨λΈμ μ ν©νλ μμΉλ₯Ό μΆμ ν μ μλ€. 거리μΌμκ° μμ§ν μ κ΅°μ΄ 2μ°¨μμμ 3μ°¨μμΌλ‘ νμ₯λκ³ ν΄μλκ° λμμ§λ©΄μ μ μ κ°μκ° ν¬κ² μ¦κ°νλ©΄μ, NDT (normal distributions transform)λ₯Ό μ΄μ©ν μ ν©μ΄ ICP (iterative closest point)μ λμμΌλ‘ λΆμνμλ€. NDTλ μ κ΅°μ λΆν¬λ‘ λ³ννμ¬ κ³΅κ°μ νννλ μμΆλ κ³΅κ° νν λ°©λ²μ΄λ€. λΆν¬μ κ°μκ° μ μ κ°μμ λΉν΄ μλ±ν μκΈ° λλ¬Έμ ICPμ λΉν΄ λΉ λ₯Έ μ±λ₯μ κ°μ‘λ€. κ·Έλ¬λ NDT μ ν© κΈ°λ° μμΉ μΆμ μ μ±λ₯μ μ’μ°νλ μ
μ ν¬κΈ°, μ
μ μ€μ²© μ λ, μ
μ λ°©ν₯, λΆν¬μ μ€μΌμΌ, λμμμ λΉμ€ λ± νλΌλ―Έν°λ₯Ό μ€μ νκΈ°κ° λ§€μ° μ΄λ ΅λ€. λ³Έ νμ λ
Όλ¬Έμμλ μ΄λ¬ν μ΄λ €μμ λμνμ¬ NDT μ ν© κΈ°λ° μμΉ μΆμ μ μ νλλ₯Ό ν₯μν μ μλ λ°©λ²μ μ μνμλ€.
λ³Έ λ
Όλ¬Έμ ννλ²κ³Ό μ ν©λ² 2κ° ννΈλ‘ λλ μ μλ€. ννλ²μ μμ΄ λ³Έ λ
Όλ¬Έμ λ€μ 3κ° λ°©λ²μ μ μνμλ€. 첫째, λ³Έ λ
Όλ¬Έμμλ λΆν¬μ ν΄νλ₯Ό λ§κΈ° μν΄ κ²½νμ μΌλ‘ 곡λΆμ° νλ ¬μ κ³ μ κ°μ μμ νμ¬ κ³΅κ°μ ννμ μ곑μ κ°μ Έμ€λ λ¬Έμ μ κ³Ό κ³ ν΄μλμ NDTλ₯Ό μμ±ν λ μ
λΉ μ μ κ°μκ° κ°μνλ©° ꡬ쑰λ₯Ό λ°μνλ λΆν¬κ° νμ±λμ§ μλ λ¬Έμ μ μ μ£Όλͺ©νλ€. μ΄λ₯Ό ν΄κ²°νκΈ° μνμ¬ κ° μ μ λν΄ λΆνμ€μ±μ λΆμ¬νκ³ , νκ· κ³Ό λΆμ°μ κΈ°λκ°μΌλ‘ μμ ν νλ₯ μ NDT (PNDT, probabilistic NDT) ννλ²μ μ μνμλ€. κ³΅κ° μ 보μ λλ½ μμ΄ λͺ¨λ μ μ λΆν¬λ‘ λ³νν NDTλ₯Ό ν΅ν΄ ν₯μλ μ νλλ₯Ό λ³΄μΈ PNDTλ μνλ§μ ν΅ν κ°μμ κ°λ₯νλλ‘ νμλ€. λμ§Έ, λ³Έ λ
Όλ¬Έμμλ μ μ‘면체λ₯Ό μ
λ‘ λ€λ£¨λ©°, μ
μ μ€μ¬μ’νμ λ³μ κΈΈμ΄λ‘ μ μνλ€. λν, μ
λ€λ‘ μ΄λ€μ§ 격μλ₯Ό κ° μ
μ μ€μ¬μ μ¬μ΄μ κ°κ²©κ³Ό μ
μ ν¬κΈ°λ‘ μ μνλ€. μ΄λ¬ν μ μλ₯Ό ν λλ‘, λ³Έ λ
Όλ¬Έμμλ μ
μ νλλ₯Ό ν΅νμ¬ μ
μ μ€μ²©μν€λ λ°©λ²κ³Ό μ
μ κ°κ²© μ‘°μ μ ν΅νμ¬ μ
μ μ€μ²©μν€λ λ°©λ²μ μ μνμλ€. λ³Έ λ
Όλ¬Έμ κΈ°μ‘΄ 2D NDTμμ μ¬μ©ν μ
μ μ½μ
λ²μ μ£Όλͺ©νμλ€. λ¨μμ
방ꡬ쑰λ₯Ό μ΄λ£¨λ κΈ°μ‘΄ λ°©λ² μΈμ λ©΄μ¬μ
방ꡬ쑰μ 체μ¬μ
방ꡬ쑰μ μ
λ‘ μ΄λ€μ§ 격μκ° μμ±νμλ€. κ·Έ λ€μ ν΄λΉ 격μλ₯Ό μ΄μ©νμ¬ NDTλ₯Ό μμ±νλ λ°©λ²μ μ μνμλ€. λν, μ΄λ κ² μμ±λ NDTλ₯Ό μ ν©ν λ λ§μ μκ°μ μμνκΈ° λλ¬Έμ λμμ κ²μ μμμ μ μνμ¬ μ ν© μλλ₯Ό ν₯μνμλ€. μ
μ§Έ, μ μ¬μ λ‘λ΄λ€μ μ κ΅° μ§λλ₯Ό NDT μ§λλ‘ μμΆνμ¬ λ³΄κ΄νλ κ²μ΄ ν¨μ¨μ μ΄λ€. κ·Έλ¬λ λ‘λ΄ ν¬μ¦κ° κ°±μ λκ±°λ, λ€κ°μ²΄ λ‘λ΄κ° λλ°λ·°κ° μΌμ΄λ μ§λλ₯Ό 곡μ λ° κ²°ν©νλ κ²½μ° NDTμ λΆν¬ ννκ° μ곑λλ λ¬Έμ κ° λ°μνλ€. μ΄λ¬ν λ¬Έμ λ₯Ό ν΄κ²°νκΈ° μνμ¬ NDT μ¬μμ± λ°©λ²μ μ μνμλ€.
μ ν©λ²μ μμ΄ λ³Έ λ
Όλ¬Έμ λ€μ 4κ° λ°©λ²μ μ μνμλ€. 첫째, μ κ΅°μ κ° μ μ λν΄ λμλλ μμ μ λ³΄κ° μ 곡λ λ μμ hueλ₯Ό μ΄μ©ν ν₯μλ NDT μ ν©μΌλ‘ κ° λμμμ λν΄ hueμ μ μ¬λλ₯Ό λΉμ€μΌλ‘ μ¬μ©νλ λͺ©μ ν¨μλ₯Ό μ μνμλ€. λμ§Έ, λ³Έ λ
Όλ¬Έμμ λ€μν ν¬κΈ°μ μμΉ λ³νλμ λμνκΈ° μν λ€μ€ λ μ΄μ΄ NDT μ ν© (ML-NDT, multi-layered NDT)μ νκ³λ₯Ό 극볡νκΈ° μνμ¬ ν€λ μ΄μ΄ NDT μ ν© (KL-NDT, key-layered NDT)μ μ μνμλ€. KL-NDTλ κ° ν΄μλμ μ
μμ νμ±νλ μ μ κ°μ λ³νλμ μ²λλ‘ ν€λ μ΄μ΄λ₯Ό κ²°μ νλ€. λν ν€λ μ΄μ΄μμ μμΉμ μΆμ κ°μ΄ μλ ΄ν λκΉμ§ μ ν©μ μννλ λ°©μμ μ·¨νμ¬ λ€μ ν€λ μ΄μ΄μ λ μ’μ μ΄κΈ°κ°μ μ 곡νλ€. μ
μ§Έ, λ³Έ λ
Όλ¬Έμ μ΄μ°μ μΈ μ
λ‘ μΈν΄ NDTκ° μ ν© κΈ°λ²μΈ NDT-D2D (distribution-to-distribution NDT)μ λͺ©μ ν¨μκ° λΉμ νμ΄λ©° κ΅μ μ΅μ μΉμ μνλ₯Ό μν λ°©λ²μΌλ‘ μ κ· NDTμ λͺ¨λΈ NDTμ λ
립λ μ€μΌμΌμ μ μνκ³ μ€μΌμΌμ λ³ννλ©° μ ν©νλ λμ μ€μΌμΌ κΈ°λ° NDT μ ν© (DSF-NDT-D2D, dynamic scaling factor-based NDT-D2D)μ μ μνμλ€. λ§μ§λ§μΌλ‘, λ³Έ λ
Όλ¬Έμ μμ€ NDTμ μ§λκ° μ¦λμ μ ν©μ μ΄μ©ν μ£Όνκ³ μΆμ λ° μ§λ μμ± λ°©λ²μ μ μνμλ€.
μ΄ λ°©λ²μ λ‘λ΄μ νμ¬ ν¬μ¦μ λν μ΄κΈ°κ°μ μμ€ μ κ΅°μ μ μ©ν λ€ NDTλ‘ λ³ννμ¬ μ§λ μ NDTμ κ°λ₯ν ν μ μ¬ν NDTλ₯Ό μμ±νλ€. κ·Έ λ€μ λ‘λ΄ ν¬μ¦ λ° μμ€ NDTμ GC (Gaussian component)λ₯Ό κ³ λ €νμ¬ λΆλΆμ§λλ₯Ό μΆμΆνλ€. μ΄λ κ² μΆμΆν λΆλΆμ§λμ μμ€ NDTλ λ€μ€ λ μ΄μ΄ NDT μ ν©μ μννμ¬ μ νν μ£Όνκ³λ₯Ό μΆμ νκ³ , μΆμ ν¬μ¦λ‘ μμ€ μ κ΅°μ νμ λ° μ΄λ ν κΈ°μ‘΄ μ§λλ₯Ό κ°±μ νλ€. μ΄λ¬ν κ³Όμ μ ν΅ν΄ μ΄ λ°©λ²μ νμ¬ μ΅κ³ μ±λ₯μ κ°μ§ LOAM (lidar odometry and mapping)μ λΉνμ¬ λ λμ μ νλμ λ λΉ λ₯Έ μ²λ¦¬μλλ₯Ό 보μλ€.The robot is a self-operating device using its intelligence, and autonomous navigation is a critical form of intelligence for a robot. This dissertation focuses on localization and mapping using a 3D range sensor for autonomous navigation. The robot can collect spatial information from the environment using a range sensor. This information can be used to reconstruct the environment. Additionally, the robot can estimate pose variations by registering the source point set with the model. Given that the point set collected by the sensor is expanded in three dimensions and becomes dense, registration using the normal distribution transform (NDT) has emerged as an alternative to the most commonly used iterative closest point (ICP) method. NDT is a compact representation which describes using a set of GCs (GC) converted from a point set. Because the number of GCs is much smaller than the number of points, with regard to the computation time,
NDT outperforms ICP. However, the NDT has issues to be resolved, such as the discretization of the point set and the objective function.
This dissertation is divided into two parts: representation and registration. For the representation part, first we present the probabilistic NDT (PNDT) to deal with the destruction and degeneration problems caused by the small cell size and the sparse point set. PNDT assigns an uncertainty to each point sample to convert a point set with fewer than four points into a distribution. As a result, PNDT allows for more precise registration using small cells. Second, we present lattice adjustment and cell insertion methods to overlap cells to overcome the discreteness problem of the NDT. In the lattice adjustment method, a lattice is expressed as the distance between the cells and the side length of each cell. In the cell insertion method, simple, face-centered-cubic, and body-centered-cubic lattices are compared. Third, we present a means of regenerating the NDT for the target lattice. A single robot updates its poses using simultaneous localization and mapping (SLAM) and fuses the NDT at each pose to update its NDT map. Moreover, multiple robots share NDT maps built with inconsistent lattices and fuse the maps. Because the simple fusion of the NDT maps can change the centers, shapes, and normal vectors of GCs, the regeneration method subdivides the NDT into truncated GCs using the target lattice and regenerates the NDT.
For the registration part, first we present a hue-assisted NDT registration if the robot acquires color information corresponding to each point sample from a vision sensor. Each GC of the NDT has a distribution of the hue and uses the similarity of the hue distributions as the weight in the objective function. Second, we present a key-layered NDT registration (KL-NDT) method. The multi-layered NDT registration (ML-NDT) registers points to the NDT in multiple resolutions of lattices. However, the initial cell size and the number of layers are difficult to determine. KL-NDT determines the key layers in which the registration is performed based on the change of the number of activated points. Third, we present a method involving dynamic scaling factors of the covariance. This method scales the source NDT at zero initially to avoid a negative correlation between the likelihood and rotational alignment. It also scales the target NDT from the maximum scale to the minimum scale. Finally, we present a method of incremental registration of PNDTs which outperforms the state-of-the-art lidar odometry and mapping method.1 Introduction 1
1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3.1 Point Set Registration . . . . . . . . . . . . . . . . . . . . . 7
1.3.2 Incremental Registration for Odometry Estimation . . . . . . 16
1.4 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
1.5 Organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2 Preliminaries 21
2.1 NDT Representation . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2 NDT Registration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.3 NDT Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.4 Transformation Matrix and The Parameter Vector . . . . . . . . . . . 27
2.5 Cubic Cell and Lattice . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.6 Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.7 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
2.8 Evaluation of Registration . . . . . . . . . . . . . . . . . . . . . . . 31
2.9 Benchmark Dataset . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3 Probabilistic NDT Representation 34
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.2 Uncertainty of Point Based on Sensor Model . . . . . . . . . . . . . . 36
3.3 Probabilistic NDT . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
3.4 Generalization of NDT Registration Based on PNDT . . . . . . . . . 40
3.5 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.5.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.5.2 Evaluation of Representation . . . . . . . . . . . . . . . . . . 41
3.5.3 Evaluation of Registration . . . . . . . . . . . . . . . . . . . 46
3.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4 Interpolation for NDT Using Overlapped Regular Cells 51
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
4.2 Lattice Adjustment . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
4.3 Crystalline NDT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.4 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
4.4.1 Lattice Adjustment . . . . . . . . . . . . . . . . . . . . . . . 56
4.4.2 Performance of Crystalline NDT . . . . . . . . . . . . . . . . 60
4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5 Regeneration of Normal Distributions Transform 65
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
5.2 Mathematical Preliminaries . . . . . . . . . . . . . . . . . . . . . . . 67
5.2.1 Trivariate Normal Distribution . . . . . . . . . . . . . . . . . 67
5.2.2 Truncated Trivariate Normal Distribution . . . . . . . . . . . 67
5.3 Regeneration of NDT . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.3.1 Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.3.2 Subdivision of Gaussian Components . . . . . . . . . . . . . 70
5.3.3 Fusion of Gaussian Components . . . . . . . . . . . . . . . . 72
5.4 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
5.4.1 Evaluation Metrics for Representation . . . . . . . . . . . . . 73
5.4.2 Representation Performance of the Regenerated NDT . . . . . 75
5.4.3 Computation Performance of the Regeneration . . . . . . . . 82
5.4.4 Application of Map Fusion . . . . . . . . . . . . . . . . . . . 83
5.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
6 Hue-Assisted Registration 91
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
6.2 Preliminary of the HSV Model . . . . . . . . . . . . . . . . . . . . . 92
6.3 Colored Octree for Subdivision . . . . . . . . . . . . . . . . . . . . . 94
6.4 HA-NDT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
6.5 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
6.5.1 Evaluation of HA-NDT against nhue . . . . . . . . . . . . . . 97
6.5.2 Evaluation of NDT and HA-NDT . . . . . . . . . . . . . . . 98
6.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
7 Key-Layered NDT Registration 103
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
7.2 Key-layered NDT-P2D . . . . . . . . . . . . . . . . . . . . . . . . . 105
7.3 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108
7.3.1 Evaluation of KL-NDT-P2D and ML-NDT-P2D . . . . . . . . 108
7.3.2 Evaluation of KL-NDT-D2D and ML-NDT-D2D . . . . . . . 111
7.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
8 Scaled NDT and The Multi-scale Registration 113
8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
8.2 Scaled NDT representation and L2 distance . . . . . . . . . . . . . . 114
8.3 NDT-D2D with dynamic scaling factors of covariances . . . . . . . . 116
8.4 Range of scaling factors . . . . . . . . . . . . . . . . . . . . . . . . . 120
8.5 Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
8.5.1 Evaluation of the presented method without initial guess . . . 122
8.5.2 Application of odometry estimation . . . . . . . . . . . . . . 125
8.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
9 Scan-to-map Registration 129
9.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129
9.2 Multi-layered PNDT . . . . . . . . . . . . . . . . . . . . . . . . . . 130
9.3 NDT Incremental Registration . . . . . . . . . . . . . . . . . . . . . 132
9.3.1 Initialization of PNDT-Map . . . . . . . . . . . . . . . . . . 133
9.3.2 Generation of Source ML-PNDT . . . . . . . . . . . . . . . . 134
9.3.3 Reconstruction of The Target ML-PNDT . . . . . . . . . . . 134
9.3.4 Pose Estimation Based on Multi-layered Registration . . . . . 135
9.3.5 Update of PNDT-Map . . . . . . . . . . . . . . . . . . . . . 136
9.4 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
9.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
10 Conclusions 142
Bibliography 145
μ΄λ‘ 159
κ°μ¬μ κΈ 162Docto