Method and device for real-time mapping and localization

• US 10,304,237 B2
• Filed: 10/10/2017
• Issued: 05/28/2019
• Est. Priority Date: 04/10/2015
• Status: Active Grant

• Current Assignee: The European Atomic Energy Community (Euratom)
• Original Assignee: The European Atomic Energy Community (Euratom)
• Inventors: Sequeira, Vitor, Wolfart, Erik, Taddei, Pierluigi, Ceriani, Simone, Sanchez-Belenguer, Carlos, Puig Alcoriza, David
• Primary Examiner(s): Crawford, Jacinta M
• Assistant Examiner(s): Doan, Phuc N
• Application Number:
  US15/729,469
• Publication Number:
  US 20180075643A1
• Time in Patent Office: 595 Days
• CPC Class Codes: G01C 21/206 specially adapted for indoor navigation
• CPC Class Codes: G01S 17/42 Simultaneous measurement of distance and other co-ordinates indirect measurement G01S17/46
• CPC Class Codes: G01S 17/89 for mapping or imaging
• CPC Class Codes: G01S 7/4808 Evaluating distance, position or velocity data
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06T 15/08 Volume rendering
• CPC Class Codes: G06T 15/10 Geometric effects
• CPC Class Codes: G06T 2200/04 involving 3D image data
• CPC Class Codes: G06T 2207/10028 Range image; Depth image; 3D point clouds
• CPC Class Codes: G06T 2215/16 Using real world measurements to influence rendering
• CPC Class Codes: G06T 7/20 Analysis of motion motion estimation for coding, decoding, compressing or decompressing digital video signals H04N19/43, H04N19/51
• CPC Class Codes: G06T 7/579 from motion

Abstract

A method for constructing a 3D reference map useable in real-time mapping, localization and/or change analysis, wherein the 3D reference map is built using a 3D SLAM (Simultaneous Localization And Mapping) framework based on a mobile laser range scanner A method for real-time mapping, localization and change analysis, in particular in GPS-denied environments, as well as a mobile laser scanning device for implementing said methods.

Claims

1. A method executed by a processor for real-time mapping, localization and change analysis of an environment, in particular in a GPS-denied environment, comprising:

(A) if no 3D reference map of the environment exists, constructing a 3D reference map of said environment by

(a) acquiring the environment with a mobile real-time laser range scanner (1) at a rate of at least 5 frames per second to provide 3D scanner data,

(b) constructing a map presentation using the 3D scanner data for each of a plurality of poses of the laser range scanner (1), each pose having an associated point cloud defined by the 3D scanner data, the map presentation having a data structure is set to natively handle 3D points and is based on a hybrid structure composed by a sparse voxelized structure used to index a compact dense list of features in the map presentation, allowing constant time random access in voxel coordinates independently from the map size and efficient storage of the data with scalability over the environment, and

(c) building, using the map presentation, the 3D reference map for the environment using a 3D Simultaneous Localization And Mapping (3D SLAM) framework, said building comprising

(i) using an odometer module, estimating a current pose of the laser range scanner (1) based on the registration of a last point cloud to a local map presentation,

(ii) using a local trajectory optimization module, refining the trajectory of a set of point clouds in order to minimize the drift in the local map presentation, and

(iii) performing offline a global trajectory optimization by reconstructing an entire map of the environment taking into account loop closures of trajectories, thereby forming said 3D reference map;

and

(B) based on an existing 3D reference map of the environment, performing real-time mapping, localization and change analysis of said environment by

(d) acquiring the environment with a real-time laser range scanner (1) at a rate of at least 5 frames per second to provide 3D scanner data,

(e) during place recognition, identifying a current location of the laser range scanner (1) inside the environment with no prior knowledge of the laser range scanner pose during place recognition, and pre-computing of simple and compact descriptors of a scene acquired by the laser range scanner (1) using a reduced search space within the scene in order to self-localize the scanner in real-time, each descriptor of the scene comprising a range image of regular bins where each bin has estimated median range value,

(f) after determination of the localization of the scanner in the environment, tracking the scanner pose by registering current scanner data inside said existing 3D reference map of the environment using standard Iterative Closest Point method employing data comprising nearest-neighbor information stored in the 3D reference map,

(g) calculating the distance between each scan point in the current scanner data and nearest point in the 3D reference map, wherein change analysis is performed by applying a threshold to this distance, whereby each point in the current scanner data which does not have a corresponding neighbor in the reference model is considered a change,

(h) displaying information about the 3D reference map and the current 3D scanner data on a real-time user interface, said information being preferably color-coded according to a change/no-change classification of said information.

2. The method of claim 1, wherein the local trajectory optimization module comprises a local window mechanism optimizing a trajectory fragment composed by a set of poses and their associated point clouds with respect to a map built up to the last registered set, wherein points are preferably converted in world coordinates using pose interpolation in 3 group and wherein a generalization of Iterative Closest Point method is preferably used to find the trajectory that better aligns all the points to the map;

wherein the local window mechanism operates such that, when the distance between the first and the last pose in the list is larger than a threshold, cloud poses are optimized and a new list is produced with the refined pose and the input clouds.

3. The method of claim 1, wherein the data structure maintains five different representations of the data stored, thereby granting consistency between internal data representations after each map update, the five representations being

(i) a (compact and dense) list of features, L and an index to the last element, L_last, where each element, l_i∈

L, contains all the information associated to a feature in the map, such as position and normal unit vector in world coordinates, and preferably additional information,

(ii) a (compact and dense) validity mask, M, where each element, m_i∈

M, is a boolean value indicating if its corresponding sample, l_i∈

L, is valid or not, ensuring that m_i=0,i>

L_last,

(iii) a list of holes, H, where each element, h_i∈

H<

L_last, indicates that l_hi is not valid so, m_hi=0,

(iv) a sparse voxel representation V, built with a parametrizable cell size, that stores in each cell, v_i∈

V, the index of its corresponding feature in L, wherein features in L and cells in V are related in a one-to-one manner, based on the position of l_i and the cell size of V, and

(v) a kd-tree, K, which is used to perform nearest neighbor searches on the map and which only stores references to the dense list L to keep its memory footprint low.

4. The method of claim 3, wherein, given an area of interest expressed by a central position and a radius, inner features are selected by looping over the elements stored in L and the kd-tree K is rebuilt as a fast mechanism for nearest neighbor searches.

5. The method of claim 1, wherein step (e) comprises the identification of a set of possible locations of the scanner based on the scanner data of step (d), said step

(e) further the following substeps:

(b1) based on the last 3D scanner data, computing an associated descriptor q and recovering a set of candidate locations Γ

by performing a radial search on T given a threshold radius r in the descriptor space, preferably for 360°

horizontal view scanner data, increasing the candidate locations by computing additional input descriptors by horizontally shifting range values, each descriptor corresponding to the readings that the scanner would produce if rotated on its local axis and then rotating according to i each resulting set of candidate locations,

(b2) associating a weight w<sub>Γ

p</sub> to each potential location Γ

_p∈

Γ

;

6. The method of claim 1, wherein step (e) comprises the identification of a set of possible locations of the scanner based on the scanner data of step (d), said step (e) further the following substeps:

(b1) based on the last 3D scanner data, computing an associated descriptor q and recovering a set of candidate locations Γ

, the candidate locations having a descriptor similar to q,

(b2) associating a weight w<sub>Γ

p</sub> to each potential location Γ

_p∈

Γ

;

7. The method of claim 6, for ground motion, whereby the laser range scanner (1) is mounted on a person or on a vehicle traversing a floor, comprising the following steps

(i) identifying in the 3D reference map the extents of the floor, wherein floor extraction is performed over a sparse voxel representation of the environment, V, where each full cell, v⁽ⁱ⁾, of the sparse voxel representation contains a normal vector to the surface locally defined by the points around its centroid, n⁽ⁱ⁾, by extracting a subset of voxels that represent candidate floor cells, F⊂

V, by checking that the vertical component in their associated normals is dominant, i.e. n⁽ⁱ⁾·

(0,0,1)^T≥

ε

, where ε

is typically a value between 0.5 and 1

(ii) determining reachability of cells, wherein given a reachable cell f∈

F, all surrounding cells (g⁽¹⁾, g⁽²⁾, . . . , g^(m))∈

F are considered as reachable if the following conditions are satisfied;

∥

f−

g⁽ⁱ⁾∥

≤

θ

₀ 

 

(6)

∥

f_z−

g_z⁽ⁱ⁾∥

≤

θ

₁ 

 

(7)

C_g(i)∩

V=Ø

 

 

(8)

where θ

₀≥

V_cellSize in (6) stands for the maximum step distance (e.g. 0.5 meters for a walking motion, or V_cellSize for a car motion), θ

₁ in (7) stands for the maximum vertical step size and C_g(i) in (8) stands for the simplified volume of the observer, centered over the floor cell g_i.

8. The method of claim 7, wherein the map structure comprises two different lists of elements that are stored and synchronized:

a compact list of planes, L, and a dense grid of voxels, V, built with a specific voxel size, each plane l_i∈

L storing a position in world coordinates, p_i, and a unit normal, n_i;

wherein each voxel, v_i∈

V stores a current state that can be either full, empty or near, full voxels storing an index to the plane l_vi∈

L, whose associated position falls into, empty cells storing a null reference and near cells storing an index to the plane l_vi∈

L whose associated position distance d_v to the voxel centre is the smallest;

preferably a near voxel is considered only if the distance d_v is under a given threshold d_max, otherwise the voxel is considered empty.

9. The method of claim 8, wherein, to improve overall system robustness, the scanner tracking is combined with an odometer, wherein

after a pose has been estimated, its associated points in world coordinates are stored into a kd-tree,

given a new acquisition by the laser range scanner (1), when a registration algorithm creates the sets of points (P_i^W), it looks for nearest neighbors in both the reference map (q_i^M, n_i^M) and in the previously fixed point cloud (q_i^on_i^o), wherein correspondences are defined as:

10. A mobile laser scanning device for real-time mapping, localization and change analysis, in particular in GPS-denied environments, arranged for implementing the method of claim 1.

11. The method of claim 10 further comprising using the mobile laser scanning device of claim 10 for 3D indoor mapping/modelling;

facility management;

accurate and real-time indoor localization and navigation;

assistance to disabled or elderly people;

design information verification;

change analysis, such as for safeguards inspections;

progress monitoring, such as for civil construction;

or disaster management and response.

12. The mobile laser scanning device of claim 10, comprising a real-time laser range scanner (1), a processing unit (3), a power supply unit and a hand-held visualization and control unit (4), wherein the real-time laser range scanner (1) is capable of acquiring the environment with a rate of at least 5 frames per second to provide scanner data, the processing unit (3) is arranged to analyze said scanner data and to provide processing results comprising 3D map/model, localization and change information to the hand-held visualization and control unit (4), which is arranged to display said processing results and to allow a user to control the mobile laser scanning device.

13. The mobile laser scanning device of claim 12, wherein the visualization and control unit (4) is a tablet computer.

14. The mobile laser scanning device of claim 13, wherein said mobile laser scanning device is a backpack (2) or vehicle mounted device.