Multi-Session, Localization-oriented and Lightweight LiDAR Mapping Using Semantic Lines and Planes

The LiDAR odometry technique is well-studied in the community. In this study, we directly use it as the front end to obtain the odometry data and raw point cloud sub-maps for each session. Specifically, keyframes of sub-maps are generated based on variations in rotation and translation. We use open-source methods to obtain semantic labels of LiDAR scans. We will introduce the settings in the experimental section. The dense semantic point clouds are used for the following feature extraction and parameterization.

Several specific types of semantic landmarks, such as poles, roads, buildings, and fences, are selected as map elements based on prior knowledge of urban scenarios. These elements commonly exist in urban environments and have compact geometric representations. In this study, poles can be represented as infinite lines, while other landmarks are represented as infinite planes. To extract these line and plane features from dense semantic point clouds, a clustering algorithm [25] and a voxel-based segmentation algorithm are used. Essentially, line and plane features are low-dimensional landmarks in this study. These line and plane features have fewer parameters compared to dense point clouds, while still preserving the original geometric information, making them beneficial for map management in large-scale urban environments.

Specifically, for line feature extraction, clustering results provide several point cloud clusters that described different pole-like objects. The direction and centroid of each object are obtained using the principal component analysis (PCA) algorithm. For plane features, the semantic point cloud is divided into voxels based on position and encoded using a hash function, which is from [14]. Points within each voxel are checked using the PCA algorithm, and plane features are extracted. The centroid and covariance matrix of each feature are saved in their host frame.

To build a lightweight mapping framework, a line observation is described using two terminal points $\mathbf{f}^{\mathcal{L}}:=\langle\mathbf{p}_{a},\mathbf{p}_{b}\rangle$, which are determined using the singular values and singular vectors from the PCA result. Similarly, a plane observation is described by the centroid and four terminal points $\mathbf{f}^{\mathcal{S}}:=\langle\mathbf{p}_{a},\mathbf{p}_{b},\mathbf{p}_{c},\mathbf{p}_{d}\rangle$, which also depended on the PCA result. The four points are the vertices of a rhombus and lie in the two directions of the singular vectors corresponding to the larger two singular values.

We then initialize and update line and plane landmarks during the online mapping procedure, where data association based on the centroid-based nearest neighbor searching method constructs the co-visibility structure. We define a line landmark $\mathbf{l}^{\mathcal{L}}:=\langle l_{s},\mathbf{c}^{\mathcal{L}},\mathbf{n}^{\mathcal{L}},\mathbf{p}^{\mathcal{L}},\left\{\mathbf{f}^{\mathcal{L}}_{i}\right\}\rangle$ and a plane landmark $\mathbf{l}^{\mathcal{S}}:=\langle l_{s},\mathbf{c}^{\mathcal{S}},\mathbf{n}^{\mathcal{S}},\mathbf{p}^{\mathcal{S}},\left\{\mathbf{f}^{\mathcal{S}}_{i}\right\}\rangle$, including a semantic label, centroid, normal, minimum parameter block, and their observation in different keyframes, similar to the visual bundle structure.

However, the point-normal form, which represents normal vectors $\bf{n}$ and random points $\bf{c}$ in Euclidean space, is over-parameterized for lines and planes and is not friendly for constructing optimization problems. To address this issue, we propose to represent an infinite line as $\mathbf{p}^{\mathcal{L}}:=\langle\alpha,\beta,{x},{y}\rangle\in\mathbb{R}^{4}$, where $\alpha$ and $\beta$ represent the direction, and ${x}$ and ${y}$ represent the offset translation on the $xOy$ plane. Similarly, an infinite plane is formulated as $\mathbf{p}^{\mathcal{S}}:=\langle\alpha,\beta,{d}\rangle\in\mathbb{R}^{3}$, where $\alpha$ and $\beta$ represent the direction, and ${d}$ represents the offset translation on the $z$-axis, which are the minimum parameter blocks mentioned before.

$$\small\mathbf{R}\left(\alpha,\beta\right)=\left[\begin{matrix}\cos\left(\beta\right)&0&-\sin\left(\beta\right)\\ \sin\left(\alpha\right)\sin\left(\beta\right)&\cos\left(\alpha\right)&\sin\left(\alpha\right)\cos\left(\beta\right)\\ \cos\left(\alpha\right)\sin\left(\beta\right)&-\sin\left(\alpha\right)&\cos\left(\alpha\right)\cos\left(\beta\right)\\ \end{matrix}\right]$$

(1)

In fact, it is possible to transform all infinite lines and infinite planes from the original line $\mathbf{u}_{z}$ and original plane $xOy$ in two steps, as demonstrated in Figure 3. Regarding line features, we first apply an offset $(x,y)$ to the original line $\mathbf{u}_{z}$ to obtain the translated line $\mathbf{l}(x,y)$. Next, we rotate the nearest point $(x,y)$ using a 2 degrees of freedom (DoF) rotation matrix $\mathbf{R}(\alpha,\beta)$ to obtain the final line $\mathbf{l}(\mathbf{R}(\alpha,\beta),x,y)$. The 2 DoF rotation matrix $\mathbf{R}(\alpha,\beta)$ is defined by equation (1). Concerning plane features, the original plane $xOy$ is translated along the $z$ axis and rotated using a 2 DoF rotation matrix, resulting in the final plane $\mathbf{s}(\mathbf{R}(\alpha,\beta),z)$.

The minimum line or plane representation is devised for the optimization framework, necessitating a residual related to the pose and landmark. While point-to-line and point-to-plane residuals are available, they are constructed using the point-normal form. Hence, it is essential to establish the mapping relations between the point-normal form and infinite line/plane form, and our proposed ones are stated as follows:

$$\small\begin{bmatrix}\mathbf{n}^{\mathcal{L}}\\ \mathbf{c}^{\mathcal{L}}\end{bmatrix}=\begin{bmatrix}\mathbf{R}(\alpha,\beta)\mathbf{u}_{z}\\ \mathbf{R}(\alpha,\beta)(\mathbf{u}_{x}x+\mathbf{u}_{y}y)\end{bmatrix}$$

(2)

$$\small\begin{bmatrix}\mathbf{n}^{\mathcal{S}}\\ d^{\mathcal{S}}\end{bmatrix}=\begin{bmatrix}\mathbf{R}(\alpha,\beta)\mathbf{u}_{z}\\ d\end{bmatrix}$$

(3)

To merge sub-maps at different locations, place recognition and relative pose estimation are critical challenges that must be solved globally, i.e., without an initial guess. Traditional methods typically employ full laser scan data to construct handcrafted or learning-based global descriptors. In our case, we implement the GraffMatch algorithm [26], which is a global descriptor-free method based on the open-sourced data association framework [27] to identify the overlapping parts between two sub-maps. However, since each sub-map contains numerous landmarks, the graph matching problem has an infeasible dimensionality, leading to unmanageable solution times.

To overcome this issue, we propose a carefully designed two-step method. Firstly, we partition the sub-map into blocks along the vehicle trajectory instead of directly matching the two sub-maps. Each block comprises a host keyframe and a set of landmarks around it. Secondly, to reduce the number of features, we cluster the plane landmarks that lie on the same infinite plane into a single plane. Specifically, we represent each pole as a line node $\mathbf{v}^{L}=\langle\mathrm{l}_{\mathrm{s}},\mathbf{c},\mathbf{n},\mathbf{l}^{L}\rangle$, and cluster the plane landmarks to form larger planes. These larger planes are represented as plane nodes $\mathbf{v}^{S}=\langle\mathrm{l}_{\mathrm{s}},\mathbf{c},\mathbf{n},\left\{\mathbf{l}_{i}^{S}\right\}\rangle$. The elements of the graph node include semantic labels, centroids, normals, and the corresponding landmark(s). The semantic graph consists of two types of nodes, namely $\mathcal{G}=\langle\left\{\mathbf{v}_{i}^{L}\right\},\left\{\mathbf{v}_{j}^{S}\right\}\rangle$.

In the GraffMatch algorithm, line and plane features are extracted to formulate the $k$-dim subspaces, and their Graff coordinates ${Y}$ are shown as (4).

$\displaystyle{Y}=\begin{bmatrix}A&b/\sqrt{\lVert b\rVert^{2}+1}\\ 0&1/\sqrt{\lVert b\rVert^{2}+1}\end{bmatrix}\in\mathbb{R}^{(n+1)\times(k+1)}$

(4)

In our situation, $A$ and $b$ are the orthonormal basis and orthonormal displacement of the line or plane extracted from pole instances and clustered plane instances. Grassmannian metric $\mathrm{d}_{\mathrm{Graff}}({Y}_{1},{Y}_{2})$ can be used to compute the distance of two subspaces, such as:

	$\displaystyle b_{02}$	$\displaystyle=b_{2}-b_{1}$		(5)
	$\displaystyle Y^{{}^{\prime}}_{1}$	$\displaystyle=\begin{bmatrix}A_{1}&0\\ 0&1\end{bmatrix}$
	$\displaystyle Y^{{}^{\prime}}_{2}$	$\displaystyle=\begin{bmatrix}A_{2}&b_{02}/\sqrt{\lVert b_{02}\rVert^{2}+1}\\ 0&1/\sqrt{\lVert b_{02}\rVert^{2}+1}\end{bmatrix}$
	$\displaystyle\left\{\sigma_{i}\right\}$	$\displaystyle=\mathrm{SVD}((Y^{{}^{\prime}}_{1})^{T}Y^{{}^{\prime}}_{2})$
	$\displaystyle\mathrm{d}_{\mathrm{Graff}}({Y}_{1},{Y}_{2})$	$\displaystyle=\ \sum_{i}{\mathrm{arccos}^{2}(\sigma_{i})}$

Then this metric is used to construct the affinity matrix for the following global matching framework. We recommend interested readers refer to CLIPPER [26] for a more detailed explanation.

Thanks to the CLIPPER framework, which gives the corrected line and plane correspondences $\mathcal{C}^{\mathcal{L}}$ and $\mathcal{C}^{\mathcal{S}}$ between semantic graphs $\mathcal{G}_{b_{i}}$ and $\mathcal{G}_{b_{j}}$ (${b_{i}}$ and ${b_{j}}$ are two individual blocks). Then we can build a hybrid registration with lines and planes to solve the relative pose, which is formulated as

	$\displaystyle\mathop{\mathrm{min}}\limits_{\mathbf{T}^{b_{i}}_{b_{j}}}$	$\displaystyle\sum_{(k,l)\in\mathcal{C}^{\mathcal{L}}}\rho(\lVert(\mathbf{I}-\mathbf{n}_{k}\mathbf{n}^{T}_{k})(\mathbf{T}^{b_{i}}_{b_{j}}\mathbf{c}_{l}-\mathbf{c}_{k})\rVert_{\Sigma_{\mathcal{L}}}^{2})$		(6)
		$\displaystyle\sum_{(k,l)\in\mathcal{C}^{\mathcal{S}}}\rho(\lVert\mathbf{n}^{T}_{k}(\mathbf{T}^{b_{i}}_{b_{j}}\mathbf{c}_{l}-\mathbf{c}_{k})\rVert_{\Sigma_{\mathcal{S}}}^{2})$

where $\mathbf{T}^{b_{i}}_{b_{j}}$ is the relative transformation between block $i$ and block $j$, while $\rho(\cdot)$ denotes a robust kernel function. However, due to the limitations of CLIPPER, it may not always be possible to find all the corresponding node pairs. Therefore, to obtain a more accurate estimate of the relative pose, an optimization-based refinement based on knn-search-based iterative closest landmark is performed, which is formulated as follows:

	$\displaystyle\mathop{\mathrm{min}}\limits_{\mathbf{T}^{f_{i}}_{f_{j}}}$	$\displaystyle\sum_{(k,l)\in\mathcal{N}^{\mathcal{L}}}\rho(\lVert(\mathbf{I}-\mathbf{n}_{k}\mathbf{n}^{T}_{k})(\mathbf{T}^{f_{i}}_{f_{j}}\mathbf{p}_{l}-\mathbf{p}_{k})\rVert_{\Sigma_{\mathcal{L}}}^{2})$		(7)
		$\displaystyle\sum_{(k,l)\in\mathcal{N}^{\mathcal{S}}}\rho(\lVert\mathbf{n}^{T}_{k}(\mathbf{T}^{f_{i}}_{f_{j}}\mathbf{p}_{l}-\mathbf{p}_{k})\rVert_{\Sigma_{\mathcal{S}}}^{2})$

where $(k,l)$ is the nearest-neighbor pair between two blocks. To achieve a more robust registration result, the optimization problem is solved iteratively for improved matching of landmarks, leading to a more stable solution. Compared to descriptor-based place recognition methods such as Scan Context [21], GraffMatch is descriptor-free but computationally more intensive. Once all pairs of blocks have been registered, we use the pairwise consistent measurement (PCM) algorithm [22] to identify false loop candidates. The function $C(\mathbf{T}^{j_{k}}_{i_{k}},\mathbf{T}^{j_{l}}_{i_{l}})$ measures the consistency of the relative pose of the block registration:

	$\displaystyle\delta\mathbf{T}$	$\displaystyle=(\mathbf{T}^{j_{k}}_{i_{k}})^{-1}\cdot\mathbf{\hat{T}}^{i_{l}}_{i_{k}}\cdot\mathbf{T}^{j_{l}}_{i_{l}}\cdot\mathbf{\hat{T}}^{j_{k}}_{j_{l}}$		(8)
	$\displaystyle C(\mathbf{T}^{j_{k}}_{i_{k}},\mathbf{T}^{j_{l}}_{i_{l}})$	$\displaystyle=[\mathbf{r}(\delta\mathbf{R}),\mathbf{r}(\delta\mathbf{p})]^{T}$
		$\displaystyle=[\lVert\mathrm{Log}(\mathbf{R}(\delta\mathbf{T}))\rVert_{2},\lVert\mathbf{p}(\delta\mathbf{T})\rVert_{2}]^{T}$

If $\mathbf{r}(\delta\mathbf{R})$ and $\mathbf{r}(\delta\mathbf{p})$ are small enough, the pair of relative pose $\mathbf{T}^{j_{k}}_{i_{k}}$ and $\mathbf{T}^{j_{k}}_{i_{k}}$ are considered to be consistent. The problem of solving the internal-consistent set of relative poses $\mathcal{L}_{\mathcal{O}}$ is a maximum clique problem that can be solved by the open-source parallel maximum clique library [28], such that all the remaining relative poses will be constructed as relative pose residual (9) and added to the following pose graph optimization.

	$\displaystyle\mathbf{r}_{\mathcal{L}_{\mathcal{O}}}\left(\mathbf{T}^{W}_{L_{k}},\mathbf{T}^{W}_{L_{k+1}},\mathbf{\hat{T}}^{L_{k}}_{L_{k+1}}\right)=$		(9)
	$\displaystyle\begin{bmatrix}{\mathbf{R}^{W}_{L_{k}}}^{T}\left(\mathbf{p}^{W}_{L_{k+1}}-\mathbf{p}^{W}_{L_{k}}\right)-\mathbf{\hat{p}}^{L_{k}}_{L_{k+1}}\\ (\mathbf{\hat{R}}^{L_{k}}_{L_{k+1}})^{T}(\mathbf{R}^{W}_{L_{k}})^{T}\mathbf{R}^{W}_{L_{k+1}}\end{bmatrix}$

Generally, based on the front-end LiDAR odometry data, the odometry pose residuals $\mathbf{r}_{\mathcal{O}}$ have the same formulation as $\mathbf{r}_{\mathcal{L}_{\mathcal{O}}}$. Thus, the classical pose graph optimization problem is formulated as

	$\displaystyle\mathop{\mathrm{min}}\limits_{\mathbf{T}^{W}_{L_{i}}\in\mathcal{F}}$	$\displaystyle\sum_{(k,k+1)\in\mathcal{O}}\rho(\lVert\mathbf{r}_{\mathcal{O}}\left(\mathbf{T}^{W}_{L_{k}},\mathbf{T}^{W}_{L_{k+1}},\mathbf{\hat{T}}^{L_{k}}_{L_{k+1}}\right)\rVert_{\Sigma_{\mathcal{O}}}^{2})+$		(10)
		$\displaystyle\sum_{(i,j)\in\mathcal{L}_{\mathcal{O}}}\rho(\lVert\mathbf{r}_{\mathcal{L}_{\mathcal{O}}}\left(\mathbf{T}^{W}_{L_{i}},\mathbf{T}^{W}_{L_{j}},\mathbf{\hat{T}}^{L_{i}}_{L_{j}}\right)\rVert_{\Sigma_{\mathcal{L}_{\mathcal{O}}}}^{2})$

Pose graph optimization provides a higher-precision global pose for keyframes and landmarks. However, it is possible to have landmarks that are repeatedly included in multiple sub-maps. To reduce the size of the map and the dimension of subsequent optimization, the instances of these landmarks in multiple sub-maps will be merged depending on either the graph-matching result or the centroid distance.

After merging the overlap landmarks between sub-maps, we introduce a new bundle adjustment formulation to jointly optimize the keyframes’ poses, line landmarks, and plane landmarks to improve the map accuracy.

Inspired by visual bundle adjustment methods, we construct the residual between the observation in the keyframe and the corresponding landmark in the world. Every line observation has two point-to-infinite-line residuals, which can be expressed as follows:

$$\small\mathbf{r}_{\mathcal{L}}\left(\mathbf{T}^{W}_{L},\mathbf{l}^{\mathcal{L}}\right)=\left(\mathbf{I}-\mathbf{nn}^{T}\right)\left(\mathbf{T}^{W}_{L}\mathbf{p}^{L}-\mathbf{q}\right)$$

(11)

where $\mathbf{T}^{W}_{L}$ is the pose of the keyframe, $\mathbf{l}^{L}$ is the line landmark whose point-normal form is $\left(\mathbf{n},\mathbf{q}\right)$, and $\mathbf{p}_{L}$ represents one of the two observation points in the keyframe.

In the same way, every plane observation has four point-to-infinite-plane residuals, such as

$$\small\mathbf{r}_{\mathcal{S}}\left(\mathbf{T}^{W}_{L},\mathbf{l}^{\mathcal{S}}\right)=\mathbf{n}^{T}\left(\mathbf{T}^{W}_{L}\mathbf{p}^{L}-\mathbf{q}\right)$$

(12)

With (2), (3), (11) and (12), we could easily calculate the jacobian matrices with respect to line parameters $\langle\alpha,\beta,x,y\rangle$ and plane parameters $\langle\alpha,\beta,d\rangle$. Then we formulate the LiDAR bundle adjustment formulation as

	$\displaystyle\mathop{\mathrm{min}}\limits_{\mathcal{X}}\sum_{(k,k+1)\in\mathcal{O}}$	$\displaystyle\rho(\lVert\mathbf{r}_{\mathcal{O}}\left(\mathbf{T}^{W}_{L_{k}},\mathbf{T}^{W}_{L_{k+1}},\mathbf{\hat{T}}^{L_{k}}_{L_{k+1}}\right)\rVert_{\Sigma_{\mathcal{O}}}^{2})+$		(13)
	$\displaystyle\sum_{(i,j)\in\mathcal{L}_{\mathcal{O}}}$	$\displaystyle\rho(\lVert\mathbf{r}_{\mathcal{L}_{\mathcal{O}}}\left(\mathbf{T}^{W}_{L_{i}},\mathbf{T}^{W}_{L_{j}},\mathbf{\hat{T}}^{L_{i}}_{L_{j}}\right)\rVert_{\Sigma_{\mathcal{L}_{\mathcal{O}}}}^{2})+$
	$\displaystyle\sum_{(k,i)\in\mathcal{C}_{\mathcal{L}}}$	$\displaystyle\rho(\lVert\mathbf{r}_{\mathcal{L}}\left(\mathbf{T}^{W}_{L_{k}},\mathbf{l}^{\mathcal{L}}_{i}\right)\rVert_{\Sigma_{\mathcal{L}}}^{2})+$
	$\displaystyle\sum_{(k,j)\in\mathcal{C}_{\mathcal{S}}}$	$\displaystyle\rho(\lVert\mathbf{r}_{\mathcal{S}}\left(\mathbf{T}^{W}_{L_{k}},\mathbf{l}^{\mathcal{S}}_{i}\right)\rVert_{\Sigma_{\mathcal{S}}}^{2})$

where $\mathcal{X}$ includes all the keyframes’ poses and landmarks.

Finally, all the multi-session poses and landmarks are optimized jointly in our proposed mapping framework. It is worth noting that the pipeline of global map merging and refinement only involves lightweight lines and planes. The effectiveness and efficiency of this map type will be validated in the following experimental sections.