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 ...

In this post, we will discuss the Iterative Closest Point (ICP) problem: from point-to-point and point-to-plane ICP to generalized ICP. Problem Formulation Let two 3D point-sets $\mathcal{X} = \{\mathbf{x}_i\}, i = 1, \ldots, N$ and $\mathcal{Y} = \{\mathbf{y}_j\}, j = 1, \ldots, M$, where $\mathbf{x}_i, \mathbf{y}_j \in \mathbb{R}^3$ are point coordinates, be the data point-set and the model point-set respectively. The goal is to estimate a rigid motion with rotation $\mathbf{R} \in SO(3)$ and translation $\mathbf{t} \in \mathbb{R}^3$ that minimizes the following $L_2$-error $E$: ...

This post discusses the assignment problem, its primal-dual interpretation, and the gated Hungarian algorithm. Tutor HungarianAlgorithm and [4] gives a nice interpretation of the dual-prime of the Hungarian algorithm. Prime-Dual Interpretation of Hungarian Algorithm The following linear program gives a lower bound on the optimal value of the assignment problem: $$\begin{array}{ll} \min & \sum_{i \in I} \sum_{j \in J} c_{i j} x_{i j} \\ \text { s.t. } & \sum_{j \in J} x_{i j}=1 \text { for all } i \in I \\ & \sum_{i \in I} x_{i j}=1 \text { for all } j \in J \\ & x_{i j} \geq 0 \end{array}$$To see this, note that we can let $x_{i j}=1$ if $i$ is assigned to $j$ and 0 otherwise. Clearly, this is a feasible solution to the L.P, so the optimal value of the LP must be at most the optimal value of the assignment problem. ...