A computer-implemented 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 G = ( V, ε) into a plurality of subgraphs in such a way that the number of edges that span different subgraph is minimized, the graph including a plurality of vertices v 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, wherein each of the native vertices is designated to be of exactly one subgraph for which it is a constrained vertex; 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 a 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, wherein the ADMM formulation is a consensus ADMM formulation given by argmin Δ 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 where k is a number of iterations, s is a number of graph nodes and T is a number of poses, Δ x k ∈ R dN denotes an updated pose, b k ∈ R dN denotes a gradient of cost functional error, H s k is a Hessian matrix of the cost functional error, wherein a positive-definite matrix E k is used to precondition an objective function of the ADMM formulation in order to prevent a convergence of the updates for the distributed Graph SLAM algorithm, and wherein the Hessian matrix H s 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, wherein the positive definite matrix E k is computed and applied in a distributed manner using the subgraphs. partitioning a graph G = ( V, ε) into a plurality of subgraphs in such a way that the number of edges that span different subgraph is minimized, the graph including a plurality of vertices v 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, wherein each of the native vertices is designated to be of exactly one subgraph for which it is a constrained vertex; 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 a 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, wherein the ADMM formulation is a consensus ADMM formulation given by argmin Δ 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 where k is a number of iterations, s is a number of graph nodes and T is a number of poses, Δ x k ∈ R dN denotes an updated pose, b k ∈ R dN denotes a gradient of cost functional error, H s k is a Hessian matrix of the cost functional error, wherein a positive-definite matrix E k is used to precondition an objective function of the ADMM formulation in order to prevent a convergence of the updates for the distributed Graph SLAM algorithm, and wherein the Hessian matrix H s 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, wherein the positive definite matrix E k is computed and applied in a distributed manner using the subgraphs.