论文题目
代码：
摘要
I. 介绍
II. 相关工作
III. 方法
- A.Motion compensation 运动补偿
- B. Geometric feature points extraction and encoding
- - 1) Dual-threshold ground filtering
  - 2) Nonground points classification based on PCA:
  - 3) Neighborhood category context encoding: 邻域类别上下文编码：
- C. Multi-metric linear least square ICP
- - 1) Multi-class closest point association:
  - 2) Multi-metric transformation estimation:
  - 3) Multi-strategy weighting function:
  - 4) Multi-index registration quality evaluation: 多指标配准质量评估：
- D. MULLS front-end
- E. MULLS back-end
IV 实验
V 结论

论文题目

MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square
MULLS：通过多度量线性最小二乘的多功能激光雷达SLAM

代码：

https://github.com/YuePanEdward/MULLS

摘要

Abstract-The rapid development of autonomous driving and mobile mapping calls for off-the-shelf LiDAR SLAM solutions that are adaptive to LiDARs of different specifications on various complex scenarios. To this end, we propose MULLS, an efficient, low-drift, and versatile 3D LiDAR SLAM system. For the front-end, roughly classified feature points (ground, facade, pillar, beam, etc.) are extracted from each frame using dualthreshold ground filtering and principal components analysis. Then the registration between the current frame and the local submap is accomplished efficiently by the proposed multimetric linear least square iterative closest point algorithm. Point-to-point (plane, line) error metrics within each point class are jointly optimized with a linear approximation to estimate the ego-motion. Static feature points of the registered frame are appended into the local map to keep it updated. For the backend, hierarchical pose graph optimization is conducted among regularly stored history submaps to reduce the drift resulting from dead reckoning. Extensive experiments are carried out on three datasets with more than 100,000 frames collected by seven types of LiDAR on various outdoor and indoor scenarios. On the KITTI benchmark, MULLS ranks among the top LiDARonly SLAM systems with real-time performance.

我们提出了MULLS，一种高效、低漂移、多功能的3D激光雷达SLAM系统。对于前端，使用双阈值地面滤波和主成分分析从每个帧中提取粗分类的特征点（ ground 地面, facade 立面, pillar 柱子, beam 梁, etc.）然后，通过提出的多尺度线性最小二乘迭代最近点算法，有效地实现了当前帧与局部子映射之间的配准。每个点类内的点到点（平面、线）误差度量通过线性近似联合优化，以估计自运动。配准帧的静态特征点被添加到局部地图中以保持其更新。对于后端，在定期存储的历史子地图之间进行分层姿态图优化，以减少航位推算导致的漂移。

I. 介绍

In this work, we proposed a LiDAR-only SLAM system (as shown in Fig. 1) to cope with the aforementioned challenges. The continuous inputs of the system are sets of 3D coordinates (optionally with point-wise intensity and timestamp) without the conversion to rings or range images.Therefore the system is independent of the LiDAR’s specification and without the loss of 3D structure information. Geometric feature points with plentiful categories such as ground, facade, pillar, beam, and roof are extracted from each frame, giving rise to better adaptability to the surroundings. The ego-motion is estimated efficiently by the proposed multimetric linear least square (MULLS) iterative closest points (ICP) algorithm based on the classified feature points. Furthermore, a submap-based pose graph optimization (PGO) is employed as the back-end with loop closure constraints constructed via map-to-map global registration.

在这项工作中，我们提出了一种仅使用激光雷达的SLAM系统（如图1所示），以应对上述挑战。系统的连续输入是一组3D坐标点（可选择具有逐点的强度和时间戳），无需转换为ring或距离图像。因此，该系统独立于激光雷达的规格，不会丢失三维结构信息。从每个框架中提取具有丰富类别的几何特征点，如地面、立面、柱、梁和屋顶，从而更好地适应周围环境。基于分类特征点，提出的多尺度线性最小二乘（MULLS）迭代最近点（ICP）算法可有效估计自运动。此外，采用基于子映射的位姿图优化（PGO）作为后端，通过map-to-map全局配准构造循环闭合约束。

The main contributions of this work are listed as follows:

A scan-line independent LiDAR-only SLAM solution named MULLS’, with low drift and real-time performance on various scenarios. Currently, MULLS ranks top 10 on the competitive KITTI benchmark2.
An efficient point cloud local registration algorithm named MULLS-ICP that realizes the linear least squareoptimization of point-to-point (plane, line) error metrics jointly in roughly classified geometric feature points.

II. 相关工作

III. 方法

A.Motion compensation 运动补偿

Given the point-wise timestamp of a frame, the time ratio for a point pip_ipi with timestamp τiτ_iτi is si=τe−τiτe−τbs_i = \frac{τ_e−τ_i}{τ_e −τ_b}si=τe−τbτe−τi , where τbτ_bτb, τeτ_eτe are the timestamp at the start and the end of the frame. When IMU is not available, the transformation of pip_ipi to piep^e_ipie can be computed under the assumption of uniform motion:

给定帧的逐点时间戳，具有时间戳τiτ_iτi的点pip_ipi的时间比为si=τe−τiτe−τbs_i = \frac{τ_e−τ_i}{τ_e −τ_b}si=τe−τbτe−τi，其中，τbτ_bτb，τeτ_eτe是帧开始和结束时的时间戳。当IMU不可用时，pip_ipi到piep^e_ipie的转换可以在匀速运动的假设下计算：

where slerp represents the spherical linear interpolation, Re,bR_{e,b}Re,b and te,bt_{e,b}te,b stands for the rotation and translation estimation throughout the frame. The undistorted point cloud is achieved by packing all the points {pie}\{p^e_i\}{pie} at the end of the frame.
其中，slerp表示球面线性插值，Re,bR_{e,b}Re,b和te,bt_{e,b}te,b表示整个帧的旋转和平移估计。无失真点云是通过将所有点{pie}\{p^e_i\}{pie}打包到帧末尾来实现的。

B. Geometric feature points extraction and encoding

The workflow of this section is summarized in Fig. 2. The input is the raw point cloud of each frame, and the outputs are six types of feature points with primary and normal vectors.
本节的工作流程如图2所示。输入是每个帧的原始点云，输出是六种类型的特征点，具有主向量和法向量

在获得激光点云后，对点云进行预处理（降采样和去畸变），分类为地面点和非地面点（将点云投影到参考平面，并在统计每个网格中点云到当前网格最低点的距离以及相邻最低点的相对距离，只要有距离大于阈值即认为该点为非地面点 NGNGNG,否则为地面点GroughG_{rough}Grough）。
对于地面点，在每个网格中使用RANSAC来拟合网格中的地面。内点被作为地面点G，它们的法向量n定义为网格中地面的法向量。

对于非地面点，基于下采样到固定数量，然后并行地输入主成分分析（PCA）模块，（基于固定半径值R，取每个点领域内最近的K个点作为该点的领域N，NNN的协方差矩阵CCC计算为 C=1∣N∣C = \frac{1}{|N|}C=∣N∣1 ∑i∈N(pi−p‾)(pi−p‾)T\sum_{i∈N} (p_i − \overline{p}) (p_i − \overline{p})^T∑i∈N(pi−p)(pi−p)T）定义局部线性度σ1Dσ_{1D}σ1D、平面度 σ2Dσ_{2D}σ2D和曲率σcσ_cσc 。σ1D=λ1−λ2λ1,σ2D=λ2−λ3λ1,σc=λ3λ1+λ2+λ3σ_{1D} = \frac{ λ_1−λ_2}{λ_1} , σ_{2D} = \frac{λ_2−λ_3}{λ_1} , σ_c = \frac{λ_3}{λ_1 +λ_2+λ_3}σ1D=λ1λ1−λ2,σ2D=λ1λ2−λ3,σc=λ1+λ2+λ3λ3 。根据局部特征 σ1Dσ_{1D}σ1D, σ2Dσ_{2D}σ2D, σcσ_cσc 的大小和v，nv，nv，n的方向，可以提取五类特征点，即立面F、屋顶R、柱P、梁B和顶点v。为了细化特征点，基于σ1Dσ_{1D}σ1D, σ2Dσ_{2D}σ2D, σcσ_cσc的非最大抑制（NMS）分别应用于线性点(P,B)(P, B)(P,B)、平面点(F,R)(F, R)(F,R)和顶点VVV，然后进行各向同性下采样。与GGG一起，对粗略分类的特征点进行打包以进行配准。

1) Dual-threshold ground filtering

The point cloud after optional preprocessing is projected to a reference plane, which is horizontal or fitted from last frame’s ground points. For non-horizontal LiDAR, the initial orientation needs to be known. The reference plane is then divided into equal-size 2D grids. The minimum point height in each grid gig_igi and in its 3×33 × 33×3 neighboring grids, denoted as hmin(i)h^{(i)}_{min}hmin(i) and hneimin(i)h^{(i)}_{neimin}hneimin(i), are recorded respectively. With two thresholds δh1δh_1δh1, δh2δh_2δh2, each point pkp_kpk in grid gig_igi is classified as a roughly determined ground point GroughG_{rough}Grough or a nonground point NGNGNG by:
可选预处理后的点云被投影到参考平面，该参考平面是水平的或从最后一帧的地面点拟合。对于非水平激光雷达，需要知道初始方位。然后将参考平面划分为大小相等的二维网格。分别记录每个网格gig_igi及其3×33×33×3相邻网格中的最小点高度，表示为hmin(i)h^{(i)}_{min}hmin(i)和hneimin(i)h^{(i)}_{neimin}hneimin(i)。通过两个阈值δh1δh_1δh1，δh2δh_ 2δh2，网格gig_igi中的每个点pkp_kpk被分类为粗略确定的地面点g｛rough｝g_｛rough｝g｛rough｝或非地面店NGNGNG，具体如下：

将点云投影到参考平面，并在统计每个网格中点云到最低点的距离 hk−hmin(i)h_k-h^{(i)}_{min}hk−hmin(i)以及相邻最低点的相对距离hmin(i)−hneimin(i)h^{(i)}_{min}-h^{(i)}_{neimin}hmin(i)−hneimin(i)，只要有距离大于阈值即认为该点为非地面点 NGNGNG,否则为地面点GroughG_{rough}Grough

To refine the ground points, RANSAC is employed in each grid to fit the grid-wise ground plane. Inliers are kept as ground points G and their normal vectors nnn are defined as the surface normal of the grid-wise best-fit planes.

为了细化地面点，在每个网格中使用RANSAC来拟合网格方向的地面。内点作为地面点G，它们的法向量nnn定义为网格最佳拟合平面的面法向量。

2) Nonground points classification based on PCA:

Non-ground points are downsampled to a fixed number and then fed into the principal components analysis (PCA) module in parallel. The point-wise K-R neighborhood N is defined as the nearest KKK points within a sphere of radius RRR. The covariance matrix CCC for NNN is calculated as C=1∣N∣C = \frac{1}{|N|}C=∣N∣1 ∑i∈N(pi−p‾)(pi−p‾)T\sum_{i∈N} (p_i − \overline{p}) (p_i − \overline{p})^T∑i∈N(pi−p)(pi−p)T, where p‾\overline{p}p is the center of gravity for NNN . Next, eigenvalues λ1>λ2>λ3λ_1 > λ_2 > λ_3λ1>λ2>λ3 and the corresponding eigenvectors v,m,nv, m, nv,m,n are determined by the eigenvalue decomposition of CCC, where v,nv, nv,n are the primary and the normal vector of N . Then the local linearity σ1Dσ_{1D}σ1D,planarity σ2Dσ_{2D}σ2D, and curvature σcσ_cσc [61] are defined as σ1D=λ1−λ2λ1,σ2D=λ2−λ3λ1,σc=λ3λ1+λ2+λ3σ_{1D} = \frac{ λ_1−λ_2}{λ_1} , σ_{2D} = \frac{λ_2−λ_3}{λ_1} , σ_c = \frac{λ_3}{λ_1 +λ_2+λ_3}σ1D=λ1λ1−λ2,σ2D=λ1λ2−λ3,σc=λ1+λ2+λ3λ3

非地面点被下采样到固定数量，然后并行地输入主成分分析（PCA）模块。逐点K-R邻域N被定义为半径为RRR的球体内最近的KKK点。NNN的协方差矩阵CCC计算为 C=1∣N∣C = \frac{1}{|N|}C=∣N∣1 ∑i∈N(pi−p‾)(pi−p‾)T\sum_{i∈N} (p_i − \overline{p}) (p_i − \overline{p})^T∑i∈N(pi−p)(pi−p)T，其中p‾\overline{p}p 是NNN的重心。接下来，通过CCC的特征值分解确定特征值λ1>λ2>λ3λ_1 > λ_2 > λ_3λ1>λ2>λ3 和相应的特征向量v,m,nv, m, nv,m,n ，其中， v,nv, nv,n 是N的主向量和法向量。然后定义局部线性度σ1Dσ_{1D}σ1D、平面度 σ2Dσ_{2D}σ2D和曲率σcσ_cσc [61]。σ1D=λ1−λ2λ1,σ2D=λ2−λ3λ1,σc=λ3λ1+λ2+λ3σ_{1D} = \frac{ λ_1−λ_2}{λ_1} , σ_{2D} = \frac{λ_2−λ_3}{λ_1} , σ_c = \frac{λ_3}{λ_1 +λ_2+λ_3}σ1D=λ1λ1−λ2,σ2D=λ1λ2−λ3,σc=λ1+λ2+λ3λ3

According to the magnitude of local feature σ1Dσ_{1D}σ1D, σ2Dσ_{2D}σ2D, σcσ_cσc and the direction of v,nv, nv,n, five categories of feature points can be extracted, namely facade F, roof R, pillar P, beam B, and vertex V. To refine the feature points, non-maximum suppression (NMS) based on σ1Dσ_{1D}σ1D, σ2Dσ_{2D}σ2D, σcσ_cσc are applied on linear points (P,B)(P, B)(P,B), planar points (F,R)(F, R)(F,R) and vertices VVV respectively,followed by an isotropic downsampling. Together with GGG, the roughly classified feature points are packed for registration.

根据局部特征 σ1Dσ_{1D}σ1D, σ2Dσ_{2D}σ2D, σcσ_cσc 的大小和v，nv，nv，n的方向，可以提取五类特征点，即立面F、屋顶R、柱P、梁B和顶点v。为了细化特征点，基于σ1Dσ_{1D}σ1D, σ2Dσ_{2D}σ2D, σcσ_cσc的非最大抑制（NMS）分别应用于线性点(P,B)(P, B)(P,B)、平面点(F,R)(F, R)(F,R)和顶点VVV，然后进行各向同性下采样。与GGG一起，对粗略分类的特征点进行打包以进行配准。

3) Neighborhood category context encoding: 邻域类别上下文编码：

Based on the extracted feature points, the neighborhood category context (NCC) is proposed to describe each vertex keypoint with almost no other computations roughly. As shown in (3), the proportion of feature points with different categories in the neighborhood, the normalized intensity and height above ground are encoded. NCC is later used as the local feature for global registration in the SLAM system’s back-end (III-E):

基于提取的特征点，提出了邻域类别上下文(NCC)来描述每个顶点关键点，几乎不需要其他计算。如(3)所示，对邻域中具有不同类别的特征点的比例、归一化强度和地上高度进行编码。NCC随后被用作SLAM系统后端（III-E）中全局配准的局部特征：

局部特征 = [领域中立面特征的比例，领域中柱子特征的比例，领域中梁特征的比例，领域中屋顶特征的比例，归一化强度，地面点高度]

C. Multi-metric linear least square ICP

This section’s pipeline is summarized in Fig. 3. The inputs are the source and the target point cloud composed of multi-class feature points extracted in III-B, as well as the initial guess of the transformation from the source point cloud to the target point cloud Tt,sguessT^{guess}_{t,s}Tt,sguess . After the iterative procedure of ICP [47], the outputs are the final transformation estimation Tt,sT_{t,s}Tt,sand its accuracy evaluation indexes.

对于每次迭代，在每个特征点类别（G、F、R、P、B、V）内的最近邻（NN）搜索来确定最近点对应关系。通过法向量n和主向量v的方向一致性进行进一步检查。通过匹配点对对应关系可以求解点到平面距离（po→plpo→plpo→pl），点到直线的距离（po→lipo→lipo→li），点到点的距离（po→popo→popo→po）。变换估计被表示为目标函数的加权最小二乘优化问题，从而可以求解出最佳的位姿估计Tt,sT_{t,s}Tt,s。P=diag(w1,⋅⋅⋅,wn)是权重矩阵，σ^\hat σσ^为后验标准差，III为信息矩阵

1) Multi-class closest point association:

For each iteration, the closest point correspondences are determined by nearest neighbor (NN) searching within each feature point category (G, F, R, P, B, V) under an ever-decreasing distance threshold. Direction consistency check of the normal vector n and the primary vector v are further applied on the planar points (G, F, R) and the linear points (P, B) respectively. Constraints on category, distance and direction make the point association more robust and efficient.

对于每次迭代，在不断减小的距离阈值下，通过在每个特征点类别（G、F、R、P、B、V）内的最近邻（NN）搜索来确定最近点对应关系。法向量n和主向量v的方向一致性检查进一步分别应用于平面点（G，F，R）和线性点（P，B）。对类别、距离和方向的约束使点关联更加鲁棒和有效。

2) Multi-metric transformation estimation:

Suppose qiq_iqi, pip_ipi are corresponding points in the source point cloud and the target point cloud, the residual rir_iri after certain rotation R and translation t of the source point cloud is defined as:
假设qiq_iqi，pip_ipi是源点云和目标点云中的对应点，在源点云的某个旋转r和平移t之后的剩余rir_iri定义为：

ri=qi−pi′r_i = q_i - p'_iri=qi−pi′
点到点距离： dipo→po=‖ri‖2d^{po→po}_ i = ‖r_i‖_2dipo→po=‖ri‖2, 点到面的距离:djpo→pl=rj⋅njd^{po→pl}_j = r_j · n_jdjpo→pl=rj⋅nj，点到线的距离： dkpo→li=‖rk×vk‖2d^{po→li}_k = ‖r_k × v_k‖_2dkpo→li=‖rk×vk‖2

根据向量积的定义，∣rk×vk∣|r_k × v_k|∣rk×vk∣就是由两个向量构成的平行四边形的面积，而这个平行四边形的面积又等于∣rk∣⋅dk|r_k|\cdot d_k∣rk∣⋅dk 则有d=∣rk×vk∣∣rk∣d=\frac{|r_k × v_k|}{|r_k|}d=∣rk∣∣rk×vk∣

With the multi-class correspondences, we expect to estimate the optimal transformation {R∗,t∗}\{R∗, t∗\}{R∗,t∗} that jointly minimized the point-to-point, point-to-plane and point-to-line distance between the vertex V, planar (G, F, R) and linear (P, B) point correspondences. As shown in Fig. 4, the point-to-point (plane, line) distance can be calculated as dipo→po=‖ri‖2d^{po→po}_ i = ‖r_i‖_2dipo→po=‖ri‖2, djpo→pl=rj⋅nj,d^{po→pl}_j = r_j · n_j ,djpo→pl=rj⋅nj, and dkpo→li=‖rk×vk‖2d^{po→li}_k = ‖r_k × v_k‖_2dkpo→li=‖rk×vk‖2 respectively from the residual, normal and primary vector (r, n, v). Thus, the transformation estimation is formulated as a weighted least square optimization problem with the following target function:

对于多类对应，我们期望估计最优变换{R∗,t∗}\{R∗, t∗\}{R∗,t∗} 这共同最小化了顶点V、平面（G，F，R）和线性（P，B）点对应之间的点对点、点对平面和点对线距离。如图4所示，点到点（平面、直线）距离可计算为 dipo→po=‖ri‖2d^{po→po}_ i = ‖r_i‖_2dipo→po=‖ri‖2, djpo→pl=rj⋅nj,d^{po→pl}_j = r_j · n_j ,djpo→pl=rj⋅nj, 和 dkpo→li=‖rk×vk‖2d^{po→li}_k = ‖r_k × v_k‖_2dkpo→li=‖rk×vk‖2 分别来自残差向量、正态向量和主向量(r，n，v)(r，n，v)(r，n，v)。因此，变换估计被表示为具有以下目标函数的加权最小二乘优化问题：

最优的旋转矩阵和平移估计 = 每个点云匹配对的距离^2 * 权重wi + 每个点云到匹配平面特征的距离^2 *权重wj + 每个点云到匹配线特征的距离^2 * 权重wk

where www is the weight for each correspondence. Under the tiny angle assumption, RRR can be approximated as:
其中，www是每个对应关系的权重。在小角度假设下，RRR可近似为：

R=Rz(ϕ)Ry(θ)Rx(ψ)=[r11r12r13r21r22r23r31r32r33]=R=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi)= \left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]=R=Rz(ϕ)Ry(θ)Rx(ψ)=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤=
[cos⁡θcos⁡ϕsin⁡ψsin⁡θcos⁡ϕ−cos⁡ψsin⁡ϕcos⁡ψsin⁡θcos⁡ϕ+sin⁡ψsin⁡ϕcos⁡θsin⁡ϕsin⁡ψsin⁡θsin⁡ϕ+cos⁡ψcos⁡ϕcos⁡ψsin⁡θsin⁡ϕ−sin⁡ψcos⁡ϕ−sin⁡θsin⁡ψcos⁡θcos⁡ψcos⁡θ]\left[\begin{array}{ccc}\cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta\end{array}\right]⎣⎡cosθcosϕcosθsinϕ−sinθsinψsinθcosϕ−cosψsinϕsinψsinθsinϕ+cosψcosϕsinψcosθcosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosϕcosψcosθ⎦⎤
当角度很小时，R=[1−ϕθϕ1−ψ−θψ1]\left[\begin{array}{ccc}1 & - \phi & \theta\\ \phi & 1& -\psi \\ -\theta & \psi & 1\end{array}\right]⎣⎡1ϕ−θ−ϕ1ψθ−ψ1⎦⎤

where α, β and γ are roll, pitch and yaw, respectively under x−y′−z′′x − y′ − z′′x−y′−z′′ Euler angle convention. Regarding the unknown parameter vector as ξ=[tx,ty,tz,α,β,γ]Tξ = [t_x ,t_y, t_z, α, β, γ]^Tξ=[tx,ty,tz,α,β,γ]T, (5) becomes a weighted linear least square problem. It is solved by Gauss-Markov parameter estimation with the functional model e=Aξ−be = Aξ − be=Aξ−b and the stochastic model D{e}=σ02P−1D\{e\} = σ^2_0P^{−1}D{e}=σ02P−1, where σ0σ_0σ0 is a prior standard deviation, and P=diag(w1,⋅⋅⋅,wn)P = diag (w_1, · · · , w_n)P=diag(w1,⋅⋅⋅,wn) is the weight matrix. Deduced from (5), the overall design matrix A∈Rn×6A ∈ R^{n×6}A∈Rn×6 and observation vector b∈Rn×1b ∈ R^{n×1}b∈Rn×1 is formulated as:

其中，α、βα、βα、β和γγγ分别为x−y′−z′′x − y′ − z′′x−y′−z′′下的roll,pitchroll, pitchroll,pitch 和 yawyawyaw。将未知参数向量视为ξ=[tx，ty，tz，α，β，γ]Tξ=[t_x，t_y，t_ z，α，β，γ]^Tξ=[tx，ty，tz，α，β，γ]T,(5) 成为加权线性最小二乘问题。基于函数模型e=Aξ−be = Aξ − be=Aξ−b和随机模型D{e}=σ02P−1D\{e\} = σ^2_0P^{−1}D{e}=σ02P−1通过高斯-马尔可夫参数估计求解，其中，σ0σ_0σ0是先前的标准偏差，P=diag(w1,⋅⋅⋅,wn)P = diag (w_1, · · · , w_n)P=diag(w1,⋅⋅⋅,wn)是权重矩阵。根据 (5) 得出，总体设计矩阵为A∈Rn×6A ∈ R^{n×6}A∈Rn×6和观测向量 b∈Rn×1b ∈ R^{n×1}b∈Rn×1 公式为：

An×6[txtytzαβγ]6×1=bn×1A_{n\times 6} \begin{bmatrix}t_x\\t_y\\t_z\\α\\β\\γ\\\end{bmatrix}_{6\times 1} =b_{n\times 1}An×6⎣⎢⎢⎢⎢⎢⎢⎡txtytzαβγ⎦⎥⎥⎥⎥⎥⎥⎤6×1=bn×1

where the components of A and b for each correspondence under point-to-point (plane, line) error metric are defined as:

eipo→po=[1000−zpiypi010zpi0−xpi001−ypixpi0][txtytzαβγ]−[xqi−xpiyqi−ypizqi−zpi]=t−r×pi−qi+pi=rke^{po→po}_ i = \begin{bmatrix}1&0&0&0& -z_{pi}&y_{pi}\\0&1&0&z_{pi}&0&-x_{pi}\\0&0&1&-y_{pi}&x_{pi}&0\end{bmatrix} \begin{bmatrix}t_x\\t_y\\t_z\\α\\β\\γ\\\end{bmatrix} - \begin{bmatrix}x_{q_i}-x_{p_i}\\y_{q_i}-y_{p_i}\\z_{q_i}-z_{p_i}\end{bmatrix} = t-r_\times pi - q_i +p_i = r_keipo→po=⎣⎡1000100010zpi−ypi−zpi0xpiypi−xpi0⎦⎤⎣⎢⎢⎢⎢⎢⎢⎡txtytzαβγ⎦⎥⎥⎥⎥⎥⎥⎤−⎣⎡xqi−xpiyqi−ypizqi−zpi⎦⎤=t−r×pi−qi+pi=rk
eipo→pl=AX−b=njT⋅t−njT⋅r×pj−njT(qj−pj)=njt(t−r×pj−qj+pj)=njtrke^{po→pl}_ i = AX-b=n^T_j \cdot t - n^T_j \cdot r_\times p_j -n^T_j(q_j -p_j) = n^t_j(t-r_\times p_j-q_j +p_j) =n^t_jr_keipo→pl=AX−b=njT⋅t−njT⋅r×pj−njT(qj−pj)=njt(t−r×pj−qj+pj)=njtrk
eipo→li=AX−b=vk×t−vk×pk×r−vk×(qk−pk)=vk×(t+r×pk−qk+pk)=rk×vke^{po→li}_ i = AX-b= v_k\times t -v_k\times p_k \times r - v_k \times (q_k - p_k) = v_k\times(t+r_\times p_k -q_k + p_k)=r_k\times v_keipo→li=AX−b=vk×t−vk×pk×r−vk×(qk−pk)=vk×(t+r×pk−qk+pk)=rk×vk

对于： (vkTpk)I3−pkvkT(v^T_kp_k)I_3 -p_kv^T_k(vkTpk)I3−pkvkT，可以反求：
rk×vk=(qk−pk′)×vk=(qk−Rpk−t)×vk=−vk×(qk−Rpk−t)=−vk×qk+vk×(Rpk+t)=−vk×qk+vk×(（r×+I）pk+t)=−vk×qk+vk×r×pk+vk×pk+vk×tr_k\times v_k = (q_k - p_k') \times v_k= (q_k - R p_k -t) \times v_k = -v_k \times (q_k - R p_k -t) =-v_k\times q_k + v_k\times(R p_k +t) =-v_k\times q_k + v_k\times(（r_\times + I） p_k +t) =-v_k\times q_k + v_k\times r_\times p_k +v_k\times p_k + v_k\times trk×vk=(qk−pk′)×vk=(qk−Rpk−t)×vk=−vk×(qk−Rpk−t)=−vk×qk+vk×(Rpk+t)=−vk×qk+vk×(（r×+I）pk+t)=−vk×qk+vk×r×pk+vk×pk+vk×t
其中 −vk×qk+vk×pk-v_k\times q_k+v_k\times p_k−vk×qk+vk×pk为−bipo→li-b^{po→li}_ i−bipo→li,vk×tv_k\times tvk×t为Aipo→liA^{po→li}_ iAipo→li左边部分,Aipo→liA^{po→li}_ iAipo→li的右边部分为vk×r×pk=−vk×pk×rv_k\times r_\times p_k = -v_k\times p_k \times rvk×r×pk=−vk×pk×r

证明： −vk×pk×-v_k\times p_k \times−vk×pk× = (vkTpk)I3−pkvkT(v_k^Tp_k)I_3-p_kv^T_k(vkTpk)I3−pkvkT
令vk=[v1,v2,v3]T,pk=[p1,p2,p3]Tv_k=[v1,v2,v3]^T,p_k=[p1,p2,p3]^Tvk=[v1,v2,v3]T,pk=[p1,p2,p3]T
−vk×pk×=−[0−v3v2v30−v1−v2v10][0−p3p2p30−p1−p2p10]=−[−v2p2−v3p3v2p1v3p1v1p2−v1p1−v3p3v3p2v1p3v2p3−v1p1−v2p2]-v_k\times p_k \times= -\begin{bmatrix}0&-v3&v2\\v3&0&-v1\\-v2&v1&0\end{bmatrix}\begin{bmatrix}0&-p3&p2\\p3&0&-p1\\-p2&p1&0\end{bmatrix}=-\begin{bmatrix}-v2p2-v3p3&v2p1&v3p1\\v1p2&-v1p1-v3p3&v3p2\\v1p3&v2p3&-v1p1-v2p2\end{bmatrix}−vk×pk×=−⎣⎡0v3−v2−v30v1v2−v10⎦⎤⎣⎡0p3−p2−p30p1p2−p10⎦⎤=−⎣⎡−v2p2−v3p3v1p2v1p3v2p1−v1p1−v3p3v2p3v3p1v3p2−v1p1−v2p2⎦⎤
(vkTpk)I3−pkvkT=[v1v2v3][p1p2p3]I3−[p1p2p3][v1v2v3]=[v1p1+v2p2+v3p3000v1p1+v2p2+v3p3000v1p1+v2p2+v3p3]−[v1p1v2p1v3p1v1p2v2p2v3p2v1p3v2p3v3p3]=[−v2p2−v3p3v2p1v3p1v1p2−v1p1−v3p3v3p2v1p3v2p3−v1p1−v2p2](v_k^Tp_k)I_3-p_kv^T_k=\begin{bmatrix}v1&v2&v3\end{bmatrix}\begin{bmatrix}p1\\p2\\p3\end{bmatrix}I_3-\begin{bmatrix}p1\\p2\\p3\end{bmatrix}\begin{bmatrix}v1&v2&v3\end{bmatrix}=\begin{bmatrix}v1p1+v2p2+v3p3&0&0\\0&v1p1+v2p2+v3p3&0\\0&0&v1p1+v2p2+v3p3\end{bmatrix}-\begin{bmatrix}v1p1&v2p1&v3p1\\v1p2&v2p2&v3p2\\v1p3&v2p3&v3p3\end{bmatrix}= \begin{bmatrix}-v2p2-v3p3&v2p1&v3p1\\v1p2&-v1p1-v3p3&v3p2\\v1p3&v2p3&-v1p1-v2p2\end{bmatrix}(vkTpk)I3−pkvkT=[v1v2v3]⎣⎡p1p2p3⎦⎤I3−⎣⎡p1p2p3⎦⎤[v1v2v3]=⎣⎡v1p1+v2p2+v3p3000v1p1+v2p2+v3p3000v1p1+v2p2+v3p3⎦⎤−⎣⎡v1p1v1p2v1p3v2p1v2p2v2p3v3p1v3p2v3p3⎦⎤=⎣⎡−v2p2−v3p3v1p2v1p3v2p1−v1p1−v3p3v2p3v3p1v3p2−v1p1−v2p2⎦⎤

点到平面残差e=(p1−p2′)⋅n1=(p1−RP2−t)⋅n1=(p1−[αβγ]×P2−p2−t)⋅n1=(p1+p2×[αβγ]−p2−t)⋅n1e = (p_1 - p'_2)\cdot n_1 = (p_1-RP_2-t) \cdot n_1 \\ = (p_1-\left[\begin{array}{ccc}\alpha\\ \beta \\ \gamma\end{array}\right]\times P_2-p_2-t) \cdot n_1 \\ = (p_1+p_2\times \left[\begin{array}{ccc}\alpha\\ \beta \\ \gamma\end{array}\right]-p_2 - t) \cdot n_1e=(p1−p2′)⋅n1=(p1−RP2−t)⋅n1=(p1−⎣⎡αβγ⎦⎤×P2−p2−t)⋅n1=(p1+p2×⎣⎡αβγ⎦⎤−p2−t)⋅n1
则有
δeδR=p2×n1;δeδt=−n1\frac{\delta e}{\delta R} =p_2\times n_1 ;\frac{\delta e}{\delta t} = -n_1δRδe=p2×n1;δtδe=−n1
AX=b=>[p2×n1−n1]X=(p1−p2)⋅n1AX=b => \left[\begin{array}{ccc}p_2 \times n_1\\ -n_1\end{array}\right]X= (p_1 - p_2)\cdot n_1AX=b=>[p2×n1−n1]X=(p1−p2)⋅n1

Therefore, the estimation of unknown vector ξ^\hat{ξ}ξ^ is:
where both ATPAA^TPAATPA and ATPbA^TPbATPb can be accumulated efficiently from each correspondence in parallel. Finally, the transformation matrix T^{R^,t^}\hat T\{ \hat R, \hat t\}T^{R^,t^} is reconstructed from ξ^\hat ξξ^.

3) Multi-strategy weighting function:

Throughout the iterative procedure, a multi-strategy weighting function based on residuals, balanced direction contribution and intensity consistency is proposed. The weight wi for each correspondence is defined as wi=wi(residual)×wi(balanced)×wi(intensity)w_i = w^{(residual)}_i × w^{(balanced)} _i × w^{(intensity)}_iwi=wi(residual)×wi(balanced)×wi(intensity) , whose multipliers are explained as follows.
在整个迭代过程中，提出了基于残差、平衡方向贡献和强度一致性的多策略加权函数。每个对应关系的权重wiw_iwi定义为wi=wi(residual)×wi(balanced)×wi(intensity)w_i = w^{(residual)}_i × w^{(balanced)} _i × w^{(intensity)}_iwi=wi(residual)×wi(balanced)×wi(intensity)，其乘数解释如下。

First, to better deal with the outliers, we present a residual weighting function deduced from the general robust kernel function covering a family of M-estimators [62]:
首先，为了更好地处理离群值，我们提出了一个残差加权函数，该函数由覆盖一系列M-估计的一般鲁棒核函数推导而来：

κκκ is the coefficient for the kernel’s shape,ϵi=di/δ\epsilon_i=d_i/δϵi=di/δ是归一化残差

where κκκ is the coefficient for the kernel’s shape, ϵi=di/δ\epsilon _i = d_i/δϵi=di/δ is the normalized residual and δδδ is the inlier noise threshold. Although an adaptive searching for the best κκκ is feasible [63], it would be time-consuming. Therefore, we fix κ=1κ = 1κ=1 in practice, thus leading to a pseudo-Huber kernel function.

其中，κκκ是核形状的系数，ϵi=di/δ\epsilon_i=d_i/δϵi=di/δ是归一化残差，δδδ为内噪声阈值。虽然自适应搜索最佳的κκκ是可行的[63]，但这将非常耗时。因此，我们在实践中固定了κ=1κ=1κ=1，从而产生了一个伪Huber核函数。

Second, the contribution of the correspondences is not always balanced in x, y, z direction. The following weighting function considering the number of correspondences from each category is proposed to guarantee observability.

其次，对应关系的贡献在x、y、z方向上并不总是平衡的。为了保证可观测性，提出了以下加权函数，其中考虑了每个类别的对应数量。

如果点是面特征（地面或屋顶），该点的balanced系数为上方的值，如果该点不是面特征，则该点的balanced系数为1

Third, since the intensity channel provides extra informa tion for registration, (12) is devised to penalize the contribution of correspondences with large intensity inconsistency.

第三，由于强度通道为配准提供了额外的信息， (12) 旨在惩罚具有较大强度不一致性的对应点的贡献。

如果δIi\delta I_iδIi接近于0时，该intensity权重为1，否则为比1小比0大的值$

4) Multi-index registration quality evaluation: 多指标配准质量评估：

After the convergence of ICP, the posterior standard deviation σ^\hat σσ^ and information matrix III of the registration are calculated as
ICP收敛后，配准的后验标准差σ^\hat σσ^和信息矩阵III计算如下：

D. MULLS front-end

As shown in Fig. 5(a), the front-end of MULLS is a combination of III-B, III-C and a map management module. With an incoming scan, roughly classified feature points are extracted and further downsampled into a sparser group of points for efficiency. Besides, a local map containing static feature points from historical frames is maintained with the reference pose of the last frame. Taking the last frame’s ego-motion as an initial guess, the scan-to-scan MULLS-ICP is conducted between the sparser feature points of the current frame and the denser feature points of the last frame with only a few iterations. The estimated transformation is provided as a better initial guess for the following scan-to-map MULLS-ICP. It regards the feature points in the local map as the target point cloud and keeps iterating until the convergence ( ξ^\hat ξξ^ smaller than a threshold). The sparser group of feature points are appended to the local map after the map-based dynamic object filtering. For this step, within a distance threshold to the scanner, those nonground feature points far away from their nearest neighbors with the same category in the local map are filtered. The local map is then cropped with a fixed radius. Only the denser group of feature points of the current frame are kept for the next epoch
如图5(a)所示，MULS的前端是III-B、III-C和地图管理模块的组合。对于传入的激光扫描，提取粗略分类的特征点，并进一步下采样为更稀疏的点云，以提高效率。此外，包含历史帧的静态特征点的局部地图与最后一帧的参考姿态保持一致。将最后一帧的自运动作为初始猜测，在当前帧的较稀疏特征点和最后一帧较密集特征点之间进行scan-to-scan MULLS-ICP，只需几次迭代。估计的变换被提供为后续扫描映射MULLS-ICP的更好的初始猜测。它将局部地图中的特征点视为目标点云，并不断迭代，直到收敛（ξ^\hat ξξ^小于阈值）。在基于地图的动态对象过滤之后，将稀疏特征点组附加到局部地图。对于该步骤，在到扫描仪的距离阈值内，过滤那些远离其在局部地图中具有相同类别的最近居的非地面特征点。然后以固定半径裁剪局部地图。下一个epoch中仅保留当前帧的更密集的特征点组。

对于传入的激光扫描，提取粗略分类的特征点，并进一步下采样为更稀疏的点云。将最后一帧的自运动作为初始猜测，在当前帧的较稀疏特征点和最后一帧较密集特征点之间进行scan-to-scan（S2S） MULLS-ICP，在基于地图的动态对象过滤之后，将稀疏特征点组附加到局部地图

E. MULLS back-end

As shown in Fig. 5(b), the periodically saved submap is the processing unit. The adjacent and loop closure edges among the submaps are constructed and verified by the certificated and efficient TEASER [46] global registration. Its initial correspondences are determined according to the cosine similarity of NCC features encoded in III-B among the vertex keypoints. Taking TEASER’s estimation as the initial guess, the map-to-map MULLS-ICP is used to refine inter-submap edges with accurate transformation and information matrices calculated in III-C. Those edges with higher σ^\hat σσ^ or lower OtsO_{ts}Ots than thresholds would be deleted. As shown in Fig. 6, once a loop closure edge is added, the pose correction of free submap nodes is achieved by the inter-submap PGO. Hierarchically, the inner-submap PGO fixes each submap’s reference frame and adjusts the other frames’ pose.

如图5（b）所示，周期性保存的子映射是处理单元。子映射之间的相邻和回环边通过经认证和有效的TEASER [46]全局配准来构造和验证。根据顶点关键点之间以III-B编码的NCC特征的余弦相似性确定其初始对应关系。将TEASER估计作为初始猜测，使用map-to-map MULLS-ICP，通过在III-C中计算的精确变换和信息矩阵来细化子映射间边缘。高于阈值的σ^\hat σσ^或低于阈值的OtsO_{ts}Ots 的边将被删去。如图6所示，一旦添加回环边，自由子映射节点的姿态校正就通过子映射PGO实现。在层次结构上，内部子映射PGO固定每个子映射的参考帧，并调整其他帧的姿势。

IV 实验

V 结论

In this work, we present a versatile LiDAR-only SLAM system named MULLS, which is based on the efficient multi-metric linear least square ICP. Experiments on more than 100 thousand frames collected by 7 different LiDARs show that MULLS achieves real-time performance with low drift and high map quality on various challenging outdoor and indoor scenarios regardless of the LiDAR specifications.
在这项工作中，我们提出了一种基于有效的多尺度线性最小二乘ICP的多功能激光雷达专用SLAM系统，称为MULLS。对7种不同激光雷达采集的10多万帧进行的实验表明，无论激光雷达规格如何，MULLS都能在各种具有挑战性的室外和室内场景中实现低漂移和高地图质量的实时性能。

MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square 论文翻译相关推荐

Lidar based off-road negative obstacle detection and analysis(论文翻译)
(机翻自己留作资料的大家辩证使用论文地址:https://ieeexplore.ieee.org/document/6083105) Abstract: 无人驾驶地面车辆 (UGV) 要想在越野 ...
SLAM综述(1)-Lidar SLAM
分享 SLAM包含了两个主要的任务:定位与构图,在移动机器人或者自动驾驶中,这是一个十分重要的问题:机器人要精确的移动,就必须要有一个环境的地图,那么要构建环境的地图就需要知道机器人的位置. 本系列 ...
SLAM综述-Lidar SLAM
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 SLAM包含了两个主要的任务:定位与构图,在移动机器人或者自动驾驶中,这是一个十分重要的问题:机器人要 ...
国内外LiDAR SLAM实验室总结
知乎上有一个关于视觉的SLAM实验室以及20年前的视觉SLAM开源算法的汇总SLAM 领域国内外优秀实验室汇总,非常详细,但是偏视觉一点,本文主要关注LiDAR SLAM. 这篇文章一开始是非常简短的 ...
Google Cartographer 《Real-Time Loop Closure in 2D LIDAR SLAM》翻译 (中英对照）
Google Cartographer <Real-Time Loop Closure in 2D LIDAR SLAM>翻译 (中英对照) 作者: Wolfgang Hess, Damo ...
3D惯导Lidar SLAM
3D惯导Lidar SLAM LIPS: LiDAR-Inertial 3D Plane SLAM 摘要本文提出了最近点平面表示的形式化方法,并分析了其在三维室内同步定位与映射中的应用.提出了一个利 ...
SLAM综述之Lidar SLAM
SLAM包含了两个主要的任务:定位与构图,在移动机器人或者自动驾驶中,这是一个十分重要的问题:机器人要精确的移动,就必须要有一个环境的地图,那么要构建环境的地图就需要知道机器人的位置. 本系列文章主要 ...
【那些年我们一起看过的论文】之《Real-Time Loop Closure in 2D LIDAR SLAM》
/* 前言:新入手2d lidar一枚,打算基于cartography进行后续开发,先对论文做一定了解. */ Step one: 先全文过一遍.有网友提供翻译结果,自己对照看.了解梗概. link: ...
Real-Time Loop Closure in 2D LIDAR SLAM 翻译和总结（一）
Hess W , Kohler D , Rapp H , et al. Real-Time Loop Closure in 2D LIDAR SLAM[C]// 2016 IEEE Internati ...

MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square 论文翻译

目录