where \(\mathbf {J}\)  is the Jacobian matrix with respect to \(\xi \) , \(\mathbf {W}\)  is a diagonal weight matrix computed by the M-estimator \(\rho \)  and residual, which is updated in each iteration. To better take advantage of the sparsity of \(\mathbf {J}\)  and \(\mathbf {W}\) , inside each Gauss-Newton step, an iterative PCG (Preconditioned Conjugate Gradient) [1]} solver is leveraged, where the diagonal matrix \(\mathbf {J}^{T}\mathbf {W}\mathbf {J}\)  is used as the preconditioner. Incremental pose updates are accumulated in the tangent space after each iteration \(\xi \leftarrow \xi \boxplus \Delta \xi \) . The entire pose graph optimization is implemented in CUDA for parallel computation.
