Scalable graph SLAM for HD maps
AssigneeRobert Bosch GmbH
InventorsNaveen Ramakrishnan, Sauptik Dhar, Jeff Irion
Life of this patent
Estimating the term…
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.
Claims6
A computer-implemented method of solving a graph simultaneous localization and mapping (graph SLAM) problem for HD maps using a distributed computing system, the method comprising:
- partitioning a graph into a plurality of subgraphs using at least one processor of the distributed computing system, 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 using the at least one processor;
- defining an alternating direction method of multipliers (ADMM) formulation for Graph SLAM based on the partitioned graph using the at least one processor;
- defining updates for the ADMM formulation based on the ADMM algorithm using the at least one processor;
- defining updates for a distributed Graph SLAM algorithm based on the updates for the ADMM formulation and in terms of the constrained and the non-constrained vertices of the subgraphs using the at least one processor;
- and
- solving the updates for the distributed Graph SLAM algorithm to solve the Graph SLAM problem for HD maps using the at least one processor to provide a distributed computing solution for the Graph SLAM problem for HD maps,
- wherein the ADMM formulation is a consensus ADMM formulation given by
- arg
-
- min
- Δ
-
- x
- 1
- k
- ,
- …
- ,
- Δ
-
- x
- N
- k
-
- ∑
- s
- =
- 1
- S
- 1
- 2
-
- (
- Δ
-
- x
- s
- k
- )
- T
-
- H
- s
- k
-
- Δ
-
- x
- s
- k
- +
- (
- b
- s
- k
- )
- T
-
- Δ
-
- x
- S
- k
- ,
- subject
-
- to
-
- Δ
-
- x
- 1
- k
- =
- Δ
-
- x
- 2
- k
- =
- …
- =
- Δ
-
- x
- N
- k
- wherein k is a number of iterations,
- wherein s is a number of graph nodes,
- wherein T is a number of the poses,
- wherein Δxk∈dN denotes an updated pose,
- wherein bk∈dN denotes a gradient of cost functional error, and
- wherein Hsk is a Hessian matrix of the cost functional error.
Dependencies are inferred from the claim text.