Bundle Adjustment for LiDAR SLAM: Mathematical Formulation and Optimization
In this post, we will discuss the post-processing of LiDAR SLAM. We mainly focus on its problem formulation. The content of this post follows papers [1] and [2]. Problem Formulation Factor graph representation of bundle adjustment formulation. (Fig. 1 of [2]) With LiDAR poses, each denoted by $\mathbf{T}_j = (\mathbf{R}_j,\mathbf{t}_j)$ $(j=1,\ldots,M_p)$, the bundle adjustment refers to simultaneously determining all the LiDAR poses (denoted by $\mathbf{T} = (\mathbf{T}_1,\cdots,\mathbf{T}_{M_p})$) and feature parameters (denoted by $\boldsymbol{\pi} = (\pi_1,\cdots,\pi_{M_f})$), such that the reconstructed map agrees with the LiDAR measurements to the best extent. Denote $c(\pi_i,\mathbf{T})$ the map consistency due to the $i$-th feature; a straightforward BA formulation is ...