Efficient Modification of the Upper Triangular Square Root Matrix on Variable Reordering

In probabilistic state inference, we seek to estimate the state of an (autonomous) agent from noisy observations. It can be shown that, under certain assumptions, finding the estimate is equivalent to solving a linear least squares problem. Solving such a problem is done by calculating the upper triangular matrix $\boldsymbol R$ from the coefficient matrix $\boldsymbol A$, using the QR or Cholesky factorizations; this matrix is commonly referred to as the 'square root matrix'. In sequential estimation problems, we are often interested in periodic optimization of the state variable order, e.g., to reduce fill-in, or to apply a predictive variable ordering tactic; however, changing the variable order implies expensive re-factorization of the system. Thus, we address the problem of modifying an existing square root matrix $\boldsymbol R$, to convey reordering of the variables. To this end, we identify several conclusions regarding the effect of column permutation on the factorization, to allow efficient modification of $\boldsymbol R$, without accessing $\boldsymbol A$ at all, or with minimal re-factorization. The proposed parallelizable algorithm achieves a significant improvement in performance over the state-of-The-Art incremental Smoothing And Mapping (iSAM2) algorithm, which utilizes incremental factorization to update $\boldsymbol R$.