Login| Sign Up| Help| Contact|

Patent Searching and Data


Title:
SCALABLE GRAPH SLAM FOR HD MAPS
Document Type and Number:
WIPO Patent Application WO/2019/162452
Kind Code:
A1
Abstract:
A method of solving a graph simultaneous localization and mapping (graph SLAM) for HD maps using a computing system includes partitioning a graph into a plurality of subgraphs, each of the subgraphs having all of the vertices of the graph and a subset of the edges of the graph. Constrained and non-constrained vertices are defined for the subgraphs. An alternating direction method of multipliers (ADMM) formulation for Graph SLAM is defined using the partitioned graph. A distributed Graph SLAM algorithm is then defined in terms of the constrained and non-constrained vertices based on the ADMM formulation. The distributed Graph SLAM algorithm is then used to solve the Graph SLAM problem for HD maps.

Inventors:
RAMAKRISHNAN, Naveen (711 Fairgate Drive, Wexford Pennsylvania, Pennsylvania, 15090, US)
DHAR, Sauptik (1063 Morse Avenue, #07-202Sunnyval, CA California, 94089, US)
IRION, Jeff (2045 W. El Camino Real, Apt. 225Mountain Vie, CA California, 94040, US)
Application Number:
EP2019/054467
Publication Date:
August 29, 2019
Filing Date:
February 22, 2019
Export Citation:
Click for automatic bibliography generation   Help
Assignee:
ROBERT BOSCH GMBH (Postfach 30 02 20, Stuttgart, 70442, DE)
International Classes:
G06F17/10; G01C21/20; G05D1/02; G06F17/12
Other References:
CHOUDHARY SIDDHARTH ET AL: "Exactly sparse memory efficient SLAM using the multi-block alternating direction method of multipliers", 2015 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS), IEEE, 28 September 2015 (2015-09-28), pages 1349 - 1356, XP032831764, DOI: 10.1109/IROS.2015.7353543
GIORGIO GRISETTI ET AL: "A Tutorial on Graph-Based SLAM", IEEE INTELLIGENT TRANSPORTATION SYSTEMS MAGAZINE, IEEE, USA, vol. 2, no. 4, 4 February 2011 (2011-02-04), pages 31 - 43, XP011329369, ISSN: 1939-1390, DOI: 10.1109/MITS.2010.939925
HUANG GUOQUAN ET AL: "Consistent sparsification for graph optimization", 2013 EUROPEAN CONFERENCE ON MOBILE ROBOTS, IEEE, 25 September 2013 (2013-09-25), pages 150 - 157, XP032548197, DOI: 10.1109/ECMR.2013.6698835
PAULL LIAM ET AL: "A unified resource-constrained framework for graph SLAM", 2016 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA), IEEE, 16 May 2016 (2016-05-16), pages 1346 - 1353, XP032908291, DOI: 10.1109/ICRA.2016.7487268
ZHAO LIANG ET AL: "Linear SLAM: A linear solution to the feature-based and pose graph SLAM based on submap joining", 2013 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS, IEEE, 3 November 2013 (2013-11-03), pages 24 - 30, XP032537391, ISSN: 2153-0858, [retrieved on 20131226], DOI: 10.1109/IROS.2013.6696327
None
Download PDF:
Claims:
CLAIMS

What is claimed is:

1. A method of solving a graph simultaneous localization and mapping (graph SLAM) problem for HD maps using a computing system, the method comprising:

partitioning a graph into a plurality of subgraphs, the graph including a plurality of vertices and a plurality of edges that extend between vertices, each of the vertices corresponding to a pose during mapping, each of the edges defining a spatial constraint between two vertices, wherein each of the subgraphs includes all of the vertices of the graphs, and wherein each of the subgraphs include a subset of the edges of the graph; defining constrained, non-constrained and native vertices for each of the subgraphs;

defining an alternating direction method of multipliers (ADMM) formulation for Graph SLAM based on the partitioned graph;

defining updates for the ADMM formulation based on the ADMM algorithm;

defining updates for a distributed Graph SLAM algorithm on the updates for the ADMM formulation and in terms of the constrained and the non-constrained vertices of the subgraphs; and

solving the updates for the distributed Graph SLAM algorithm to solve the Graph SLAM problem.

2. The method of claim 1 , wherein the ADMM formulation is a consensus ADMM formulation given by

argmin subject to Axk=Axk = = Dc^

Dc^,. .,Dc^ where A is a number of iterations, s is a number of graph nodes and T is a number of poses.

3. The method of claim 2, wherein the updates for the consensus ADMM

formulation are given by

4. The method of claim 3, wherein := Dsvl e UdN, and zsl = Dszl e UdN define restriction of entries of v; and zl, respectively, that are constrained in subgraph Gs, wherein a primal residual for subgraph Gs is defined as f5i = Axsl - zl e RdN, and wherein f5i = Axsl - zsl = E>5fI e RdN denotes restriction to entries that correspond to constrained vertices.

5. The method of claim 4, wherein the updates for the distributed Graph SLAM algorithm are defined in terms of constrained and non-constrained vertices such that

f1 + 1zl+ 1 _ zl

vl+1 = (1— a)vl— fi+1

6. The method of claim 1 , wherein the computing system comprises a distributed storage and computing system where the data resides in multiple nodes, and the computation is distributed among those nodes.

7. The method of claim 6, wherein the computing system has an Apache Spark framework with data stored on a Hadoop Distributed File System ( HDFS ).

Description:
SCALABLE GRAPH SLAM FOR HD MAPS

CROSS-REFERENCE TO RELATED APPLICATIONS

[0001] This application claims priority to U.S. Provisional Application Serial No.

62/634,327 entitled " SCALABLE GRAPH SLAM FOR HD MAPS" by Irion et al., filed February 23, 2018, the disclosure of which is hereby incorporated herein by reference in its entirety.

TECHNICAL FIELD

The present disclosure is directed to systems and methods for solving Graph SLAM problems, and, in particular, to systems and methods for solving Graph SLAM problems for HD maps.

BACKGROUND

[0002] The aim of Graph SLAM is to find the set of poses {x-i, x 2 _ XA/} that minimizes the cost functional

where x = (xi , xj, ... ,c^)· This is typically solved in an iterative manner whereby each iteration k entails approximating the cost functional, finding the optimal set of pose updates Ax k , and computing the next set of poses via

k + 1 = x fe EE Ax k , (2) where EE denotes a pose composition operator. Let d denote the dimensionality of the pose updates (i.e., Ax k e R dN ). Substituting (2) into (1 ), we compute the gradient b fe e U dN and the Hessian matrix H fe e dNxdN of the x 2 error as

[0003] The updates x k are obtained by linearizing the edges as

and substituting into (3), yielding the quadratic approximation

This is minimized by

[0004] Most previously known solvers solve this linear system iteratively on a single node. However, such single-node solvers do not scale well with the size of the mapping region. This necessitates the need for scalable systems and algorithms to build HD Maps for larger areas.

SUMMARY

[0005] In accordance with one embodiment of the present disclosure, a method of solving a graph simultaneous localization and mapping (graph SLAM) for HD maps using a computing system includes partitioning a graph into a plurality of subgraphs, each of the subgraphs having all of the vertices of the graph and a subset of the edges of the graph. Constrained and non-constrained vertices are defined for the subgraphs An alternating direction method of multipliers (ADMM) formulation for Graph SLAM is defined using the partitioned graph. A distributed Graph SLAM algorithm is then defined in terms of the constrained and non-constrained vertices based on the ADMM formulation. The distributed Graph SLAM algorithm is then used to solve the Graph SLAM problem for FID maps.

BRIEF DESCRIPTION OF THE DRAWINGS

[0006] FIG. 1 (a) shows how the vertices of subgraphs G 1 and G 2 of graph G s are defined;

[0007] FIG. 1 (b) shows the constrained vertices of subgraphs G 1 and G 2 .

[0008] FIG. 2 is a schematic depiction of a computing system for implementing the distributed Graph SLAM algorithm for FID maps described herein.

DETAILED DESCRIPTION

[0009] For the purposes of promoting an understanding of the principles of the disclosure, reference will now be made to the embodiments illustrated in the drawings and described in the following written specification. It is understood that no limitation to the scope of the disclosure is thereby intended. It is further understood that the present disclosure includes any alterations and modifications to the illustrated embodiments and includes further applications of the principles of the disclosure as would normally occur to a person of ordinary skill in the art to which this disclosure pertains. [0010] There are a variety of frameworks available for large-scale computation and storage. Such frameworks can be broadly categorized as,

a. Single-node in-memory systems, where the entire data is loaded and processed in the memory of a single computer.

b. In-disk systems, where the entire data resides in disk, and chunks of it are loaded and processed in memory. Although there are not many available tools that adopts this approach for solving (1 ), most of the single-node solutions can be extended through the standard in-disk capabilities available in standard computation platforms. For example, memmapfile in MATLAB, mmap in Python, etc.

c. In-database systems, where the data is stored in a database and the processing is taken to the database where the data resides.

d. Distributed Storage and Computing systems, where the data resides in multiple nodes, and the computation is distributed among those nodes. Typical frameworks involve, Hadoop, Apache Spark (distributed in-memory analytics with storage on HDFS).

[0011] In one embodiment, the Apache Spark computing framework with the data stored in FIDFS is used to implement the scalable Graph SLAM system. This offers several advantages. Firstly, compared to the existing single-node based solutions, the distributed architecture allows to scale-out the computation for large scale mapping. Further, compared to other distributed solutions like Fladoop; Spark provides a distributed in-memory computation with lazy execution. This makes it very fast and fault- tolerant, especially for iterative tasks compared to Fladoop(Map-Reduce) which needs to write all intermediate data to the disk. In alternative embodiments, any suitable computing framework may be used.

[0012] Utilizing such distributed architectures necessitates the adoption of advanced optimization algorithms that can handle the problem (1 ) at scale. Basically, the problem in (1 ) entails solving a quadratic program (QP) iteratively. Recent research towards solving such QPs in a distributed setting typically adopts one of the following

approaches:

a. First Order methods, which use first order information from the objective like., gradient estimates, secant approximates etc. Such methods incur low per- iteration computation complexity and provide dimension independent

convergence. However, the convergence rates heavily depend on the step-size selection and the conditionality of the problem.

b. Second Order methods, which use additional curvature information like, Hessian (or approximations like L-BFGS), and improves the conditionality of the problem. Such methods provide improved convergence rates but results to worse per-step computation costs.

c. Randomized algorithms, which use a subsampled or low rank representation of the original large scale data to solve a small scale equivalent problem.

[0013] Another broader class of algorithms adopt a splitting technique, which decouples the objective function into smaller sub-problems and solves them iteratively. The resulting sub-problems are easier to tackle and can be readily solved in a distributed fashion using the approaches discussed above. Some such popular algorithms are, Proximal Gradient methods, Proximal (quasi-) Newton methods, Alternating Direction Method of Multipliers (ADMM), etc. Of these approaches ADMM has been widely adopted to solve several large scale practical problems. There are several adaptations of the ADMM algorithm customized to utilize the structure of the problem. One such version well-suited for parallel and distributed settings is consensus ADMM.

[0014] Consensus ADMM in it’s basic form solves the following problem,

Wi ,.„,Ws,Z

subject to w s = z Vs = l ... S.

[0015] The consensus ADMM steps are

Here, I Wl . Ws (z) = {z|Wi = ··· = w 5 = z]

[0016] Note that, such a form is well-suited for distributed settings using the map- reduce paradigm in Spark. In this case, the w-step involves distributing (mapping) the computation over several nodes vs = 1 . . . S. And the (consensus) z - step involves aggregating (reducing) the distributed computations at the master node. Further, for (1 ), the w 5 -update entails a closed form solution, and can be efficiently computed using standard linear solvers. Although (10) is guaranteed to converge, the convergence rate of the algorithm heavily depends on the conditionality of the problem (1 ), and the selection of the parameter p.

[0017] In order to utilize consensus ADMM for Graph SLAM, the graph G = ( V, £ ) into a set of subgraphs {G t , G 2 , ... , (¾ where each subgraph includes a number of vertices V such that = V 2 = ··· = V s = V and the sets of edges £ satisfy u =1 £ s = £ and £ s n £ t = 0 if s ¹ t. Define

[0018] Observe that the gradient and the Hessian matrix, respectively, are given by

[0019] Under the condition that observe that (17) is equal to (7). Thus, our consensus ADMM formulation for Graph SLAM is

argmin

Dc^,. .,Dc^ subject to Ax k =Ax k = ··· = Dc^, and the resulting over-relaxed ADMM updates are

u k +1 = u k 1 + aAx k,l+1 + (1— a)z k 1 — z k,l+1 . (21 )

Henceforth, the superscript k will be omitted on the ADMM variables Dc, z, and u, and it should be understood that the ADMM iterations are performed as part of the k th Graph SLAM iteration.

[0020] While each subgraph G s contains all N vertices (i.e., V s V), it contains only a subset of the edges (i.e., £ s £). With that in mind, we define three different classes of vertices (see Figure 1 a for an illustration).

1 ) A constrained vertex of subgraph G s is a vertex which is constrained by at least one edge e e £ s . Let N s denote the number of constrained vertices in subgraph G s . The entries in Dc^ and that correspond to constrained vertices are denoted as Ax s l e M dWs and u* e M dWs , respectively, and the corresponding entries are denoted by b k e R dNs and H k e

M dWsXdWs . Furthermore, let D k e dWsXdw denote the matrix which down-samples to the entries constrained in subgraph G s (i.e., D s Ax s l = Ax s l e M dWs and DjAx s l e M dw ).

2) A non-constrained vertex of subgraph G s is a vertex which is not constrained by any edges in £ s . Let D s e M d( w - ws)xdw denote the matrix which down-samples to the entries not constrained in subgraph G s . Let v l e M dw denote the values assumed by the non-constrained entries in u^, ... , u ·; in other words, all s.

3) When partitioning the graph, each vertex is designated to be a native vertex of exactly one subgraph for which it is a constrained vertex; when multiple subgraphs contain edges that constrain a vertex, the choice of its native subgraph must be decided. V s native to denote the set of vertices which are native to subgraph G s .

[0021] A simplified example of graph partitioning is depicted in FIG. 1. The graph G is partitioned into subgraphs G 1 and G 2 . The graph G includes four columns and four rows. Referring to FIG. 1 (b), the first subgraph G 1 contributes to rows and columns 1 -3 of the Flessian, and the second subgraph G 2 contributes to rows and columns 3 and 4. The darker shaded regions indicate native, constrained vertices for subgraph G 1 and the lighter shaded areas indicate native, constrained vertices for subgraph G 2 . The subgraphs’ non-constrained vertices are not labeled.

[0022] To simplify notation, let us define v s l - = D s v l e R dN , and z s l = D s z l e R dN , as the restriction to the entries of v ; and z l , respectively, that are constrained in subgraph G s . Let us define a primal residual for subgraph G s as

FI := Ax s l - z l E R dN , (22)

and, as before, let

denote the restriction to entries that correspond to constrained vertices.

[0023] With these definitions in place, we can now break down the updates in terms of constrained and non-constrained vertices and present our distributed algorithm for solving Graph SLAM via ADMM:

[0024] The algorithm presented above will converge... To obtain a symmetric, positive-definite matrix E k which can be used to precondition the objective function of (18) as

[0025] The matrix E k should be selected such that it can be computed and applied in a distributed manner using the subgraphs. Preferably, the term E k El k E k =

E k El k E k « /. In order to achieve this, the Hessian El k is approximated for each subgraph using only contributions to the Hessian in rows and columns that correspond to vertices which are native to the subgraph (see FIG. 1 (b). For example, suppose e i7 - is a binary edge which constrains vertices v t and v j . Let Axf = 1* QAx k , where Q denotes element-wise multiplication and 1* e R dN denotes a vector which is 1 in entries that correspond to vertex v t and 0 elsewhere, and define Axf accordingly. The edge may be defined as

where d is the Kronecker delta. Note that e i7 (x) = e [i ] (x), so the 2 error and optimization problem remain unchanged. The native Hessian H k native for a subgraph is defined as

[0026] Note that the supports of H k native , ... , H k native are disjoint. This allows for fast, distributed computations, and so the preconditioning matrix is defined as

,k _

E k = (åf_ ! s, native native ^ 2 . (35)

By redefining the edges, as in (33), more constraints can be accounted for which in turn enables the Hessian to be better approximates, yielding a better preconditioning matrix.

[0027] It is worth mentioning that a subgraph’s Hessian H k and its native Hessian H s, native can both contain contributions that the other lacks. This is illustrated in FIG. 1 , where edge e 23 e £ 1 constrains vertices v 2 e ^ native and v 3 e V 2 native . The Hessian H k contains contributions from edge e 23 in the (2, 2), (2, 3), (3, 2), and (3, 3) blocks, but e 23 only contributes to the (2, 2) block of Hi ,native . On the other hand, the native Hessian 2 , native contains the (3, 3) block contribution from e 23 but H k does not contain any contributions from e 23 . As vertices v 2 and v 3 are native to different subgraphs, the (2, 3) and (3, 2) block contributions from edge e 23 do not contribute to any native Hessians - such is the price that must be paid to ensure that the algorithm remains distributed. This also demonstrates the importance of partitioning the graph in such a way that the number of edges that span different subgraphs is minimized.

[0028] FIG. 2 depicts an exemplary system 100 for implementing the scalable Graph SLAM framework described above. The system 100 includes a processing system 102 that is operatively connected to a memory system 104. The processing system 102 includes one or more processors which may be located in the same device/location or may be distributed across multiple devices at one or more locations. Any suitable type of processor(s) may be used.

[0029] The processing system is operably connected to the memory system 104.

The memory system 104 may include any suitable type of non-transitory computer readable storage medium. The memory system 104 may be distributed across multiple devices and/or locations. Programmed instructions are stored in the memory system. The programmed instructions include the instructions for implementing the operating system and various functionality of the system. The programmed instructions also include instructions for implementing the scalable Graph SLAM and ADMM algorithms described herein.

[0030] The components of the system may be communicatively connected by one or more networks 106. Any suitable type of network(s) may be used including wired and wireless types of networks. The computing devices include the appropriate network interface devices that enable transmitting and receiving of data via the network(s).

[0031] While the disclosure has been illustrated and described in detail in the drawings and foregoing description, the same should be considered as illustrative and not restrictive in character. It is understood that only the preferred embodiments have been presented and that all changes, modifications and further applications that come within the spirit of the disclosure are desired to be protected.