Autoware.ai
Source
Edit
History

3D-NDT-匹配算法

Catalogue
  1. 1. 1. 3D-正态分布变换(The Normal Distributions Transform)
  2. 2. 2. The normal-distributionstransform
  3. 3. 3. NDT扫描匹配
  4. 4. 4. 3D-NDT
  5. 5. 5. 一些测试评估
  6. 6. 6. Code And Result
    1. 6.1. 6.1. 结果
      1. 6.1.1. 6.1.1. 参数1 ， Cell分辨率为1m
      2. 6.1.2. 6.1.2. 参数2 ， Cell尺寸 2m
  7. 7. 7. 扩展
  8. 8. 8. 参考资料

1. 3D-正态分布变换(The Normal Distributions Transform)

前面探讨过2D-NDT匹配算法的原理和推导以及代码阅读，接下来将这个思想扩展到3D，这个工作可参见如下博士论文:The Three-Dimensional Normal-Distributions Transform— an Efficient Representation for Registration,Surface Analysis, and Loop Detection

该论文中，关于NDT的描述，从第六章开始

2. The normal-distributionstransform

使用点云来表示表面有许多限制，举例如点云不包含关于表面特征(如方向、平滑度或孔洞)的显式信息，根据传感器的配置，点云也可能是低效的，需要不必要的大量存储，为了在远离传感器位置的地方获得足够的样本分辨率，通常需要以一种从传感器附近的表面产生大量冗余数据的方式配置传感器。

正态分布变换可以看做是描述平面的一种方法，首先被 Biberand和 Straße在2003年提出用于2D扫描匹配。后来与Sven Fleck 联合，讨论了在扫描匹配和建图方面的扩展。该变换将点云映射到平滑的表面表示，描述为一组局部概率密度函数(PDFs)，每一个都描述了一段表面的形状。

该算法的第一步是将扫描程序占用的空间细分为网格单元格(在2D情况下为正方形，在3D情况下为立方体)。为每一个cell都计算一个概率密度函数PDF。每一个cell的概率密度函数PDF可以被解释为生成一个在cell里面的平面点\(\vec{x}\)的过程。换句话说，它假设\(\vec{x}\)的位置是被这个分布采样出来的。

假设参考点的位置是由一个D维的正态分布随机过程产生的，那么\(\vec{x}\)被观测的可能性是：

其中，\(\vec{\mu}\)和\(\Sigma\)分别代表参考平面点在\(\vec{x}\)所在的cell的这个分布的均值和协方差矩阵。

前面的系数可以整合成一个常数，常用\(c_0\)代替，那么均值和协方差可以表示成如下：

其中，\(\vec{y}_{k=1,\cdots,m}\)是参考帧在这个cell里面的扫描点。

NDT 给出了点云的可微分的平滑表示，每一个概率密度函数PDF可以看做是可以看作是局部表面的近似化，描述表面的位置以及它的方向和平滑度。

一帧2D扫描以及敌营的NDT如下图6.1，另外图6.2展示了一帧在矿洞隧道扫描的NDT

下面从单变量正态分布讲起，一维的随机变量\(x\)具有期望\(\mu\)，该不确定性使用\(\sigma\)来表示：

在多维的情况下，协方差矩阵的对角元素表示每个变量的方差，非对角元素表示变量的协方差，下图6.3展示了一维、二维、三维的正态分布

在二维和三维的情况下，可以通过协方差矩阵的特征向量和特征值来评估曲面的方向和平滑度

  • 特征向量描述了分布的主成分，即与各变量协方差的主导方向对应的正交向量的集合
  • 根据方差的比例，二维正态分布可以是点状的(如果方差相似的话)，也可以是线状的(如果一个变量的方差比另外一个大很多)，又或者是两者之间
  • 在三维情况下，如图6.4展示的
    • 正态分布可以描述点或球(如果方差的大小在各个方向上是相似的)，
    • 或者是一条线(如果一个方向的方差远远大于另两个方向)，
    • 或者是一个平面((如果一个方向的方差比另两个方向的方差小得多))

3. NDT扫描匹配

当使用NDT进行扫描配准时，目标是找到当前扫描点最大可能位于参考扫描面上的位置。

需要被优化的参数有：当前帧扫描的位姿估计的旋转和平移，可以使用向量\(\vec{p}\)来表示(pose)。

当前帧扫描可以使用点云\(\mathcal{X}={\vec{x_1},\cdots,\vec{x_n}}\)来表示。假设存在空间变换函数\(T(\vec{p},\vec{x})\)，可以把当前帧坐标系下的点\(\vec{x}\)通过当前位姿\(\vec{p}\)映射到世界坐标系。对扫描点给出一些概率密度函数PDF\(p\vec{x}\)，那么最佳的位姿\(\vec{p}\)应该是使下面的似然函数最大化：

或者最小化带负号的似然函数

概率密度PDF不一定非得是正态分布，任何一种局部捕获表面点结构并对异常值具有鲁棒性的pdf都是合适的。当点远离均值时，正态分布的负对数可能是无限的增加，因此，扫描数据中的异常值可能对结果有很大的影响。 Biber, Fleck,and Straße的工作中，使用了高斯模型和均匀分布的混合，

其中,\(p_o\)是预计outiler的比率。使用这个函数，outliers对于函数的影响是有限的，这个结果如下图6.5展示。常数\(c_1\)和\(c_2\)can be determined by requiring that the probability mass of ̄p(~x) equals onewithin the space spanned by a cell.

要优化的对数似然函数具有这样的形式：\(-\log(c_1 \exp (-[(\vec{x}-\vec{\mu})^T\Sigma^{-1}(\vec{x_1}-\vec{\mu})]/2)+c2)\)，它们没有简单的一阶和二阶导数，然而，图6.5(b)表明，对数似然函数又可以近似为高斯函数。即可以使用高斯模型\(\tilde{p}(x)=d_1 \exp (- d_2 x^2 / (2 \sigma^2))+d_3\)来近似，其中，参数\(d_1,d_2,d_3\)需要使用下式来计算得到：

使用这种高斯近似，当前帧扫描的一个点对NDT评分函数的影响是：

其中，\(\mu_k\)和\(\Sigma_k\)是点\(\vec{x_k}\)所在的cell的NDT变换的均值和方差。这个NDT评分函数具有比式6.7更加简单的微分形式，但在优化时仍然表现出相同的一般属性。注意，方程6.9中省略了\(d_3\)项，当使用NDT进行扫描匹配时，它不是必需的，因为它只是给评分函数增加一个常量偏移量。

给定一个点集\(\mathcal{X}={\vec{x_1},\cdots,\vec{x_n}}\)，以及一个位姿\(\vec{p}\)和一个变换函数\(T(\vec{p},\vec{x})\)，将点\(\vec{x}\)使用位姿\(\vec{p}\)进行变换，当前参数向量的NDT评分函数\(s(\vec{p})\)为:

似然函数需要协方差矩阵的逆\(\Sigma^{-1}\)。如果一个cell中的点是完全共面或共线的，则协方差矩阵是奇异的，不能求逆。在3D的情况下，由三个或更少的点组成的协方差矩阵总是奇异的。因此，PDFs仅计算包含5个以上点的cell，此外，需要警惕数值问题，为了防止协方差矩阵奇异，如果协方差矩阵\(\Sigma\)最大的特征值\(\lambda_3\)比其他两个特征值\(\lambda_1,\lambda_2\)大100倍，那么最小的特征值应该使用最大特征值的1/100来替换，即\(\lambda_j'=\lambda_3/100\)。

牛顿迭代法通常求解增量方程\(\boldsymbol{H\Delta \vec{p}=-\vec{g}}\)，其中\(\boldsymbol{H}\)和\(g\)是评分函数\(s(\vec{p})\)的海森矩阵和梯度向量。求得的增量用来更新优化位姿变量,\(\vec{p}\leftarrow\vec{p}+\Delta \vec{p}\)

简要来说，令\(\vec{x}_k'\equiv T(\vec{p},\vec{x}_k)-\vec{\mu}_k\)，也就是\(\vec{x}_k‘\)是点\(\vec{x}_k\)变换之后的点。

那么梯度向量\(\vec{g}\)中的每一个元素\(g_i\)可以如下表示：

海森矩阵\(\boldsymbol{H}\)中的每个元素\(H_{ij}\)是

NDT评分函数的梯度(6.12)和Hessian(6.13)以相同的方式表示，无论匹配是在2D还是3D中执行。它们同样独立于所使用的转换表示，另一方面，方程6.12和6.13中\(\vec{x}'\)的一阶和二阶偏导数依赖于变换函数\(T()\)。

总结： 下面的算法2流程，描述了整个NDT的匹配过程

4. 3D-NDT

2D与3D的NDT配准的主要区别在于空间变换函数\(T(\vec{p}，\vec{x})\)及其偏导数。在三维的情况下，有几种可能的方法来表示旋转。

在之前对3D-NDT的研究中[63,66]，使用了轴/角旋转表示。然而，这样做会给优化问题增加一个额外的变量，并且需要额外的约束来保持旋转轴对称的单位长度，牛顿优化法是一种迭代法，在每次牛顿迭代后，只需将旋转表示重新归一化，就可以实现对单位轴的约束。然而，当牛顿更新方向偏离到位姿参数空间的不可行区域时，这种策略仍然会导致问题，这也许可以解释早期结果中的一些不一致。为了完整起见，附录中提供了轴/角变换函数及其导数。

接下来，3D欧拉角用于描述旋转，尽管这种旋转表示法有潜在的问题。(ps: 个人想法：为什么不用李群李代数来表示位姿？se3)。优点是数值优化过程不需要约束，导数稍微简单一些，这样做会产生Gimbal Lock的风险，这以为着在大角度的情况下，配准可能会失效。利用欧拉角，有六个转换参数需要优化:三个平移量，三个旋转量，可使用6维向量表示\(\vec{p}_6=[t_x,t_y,t_z,\phi_x,\phi_y,\phi_z]^T\)。

使用(z-y-x)321欧拉角顺序，其旋转矩阵可参见：MIT材料

上图中，旋转矩阵\(T_{0,3}\)表示了从载体坐标系到参考坐标系的旋转变换

因此，使用欧拉角的变换函数为：

描述了从当前帧的点到参考帧的变换。

上式6.17对第i维优化变量的一阶偏到对于与雅克比\(\frac{\delta T_E(\vec{p}_6,\vec{x})}{\delta p_i}\)矩阵的第i列，雅克比矩阵可以写成如下形式：(一般在SLAM里面，通常写成行的形式，也就是下面的\(J_E\)的转置形式，即\(J_E^T\))

那么二阶导\(\frac{\delta ^2 T_E(\vec{p}_6,\vec{x})}{\delta p_i \delta p_j}\)

如果旋转角度比较小，那么利用下面的小角度三角近似法可以大大简化计算

对于小于\(10\degree\)的角，可以认为这些近似是精确的，对于正弦函数，在角度为\(14\degree\)时近似误差达到1%，对于余弦函数，同样的误差发生在\(8.2\degree\)上。利用小角度方法计算变换函数及其导数的速度较快，但是，与在某些情况下使用6.17方程相比，用近似进行配准的鲁棒性要差一些。附录B.1提供了相应的小角度近似值变换函数，在时间要求严格的应用中，建议采用近似公式，尽管差别不大。

用于非线性优化的许多应用，通常实用Hessian的数值近似来代替严格Hessian，因为海森矩阵是很难进行解析计算的，因为计算成本太高。然而，由于NDT评分函数的海森矩阵可以被分析计算，并且它的大部分元素为零，不使用近似值反而是有利的。

5. 一些测试评估

结论

Cell Size：

  • 在使用NDT时，选择合适的Cell大小是很重要的，如果Cell太大，单个Cell的结构就不能通过实用单个高斯模型很好地描述，如果Cell太小，那么得到的高斯模型很有可能受噪声影响很大。
  • 对于稀疏采样的点云，如果Cell很小，每个Cell内的点可能太少，无法计算出可靠的密度函数
  • 最佳单元格大小主要取决于输入数据的规模，如果使用Sick激光雷达，Cell的尺寸应该在1m到3m比较好。如果使用3D相机，由于传感器捕获的视野很窄，距离很短，使用这么大的Cell显然是不合适的，通过实验发现，这种情况下Cell的尺寸应该选择0.5m到0.75m
  • 对于具有大规模特征的扫描对，可以使用较大的单元而不牺牲精度

最好的Cell尺寸取决于扫描的范围以及扫描中结构的数量，对于lidars在移动机器人应用程序中获取的尺度扫描，1m到2mare之间的Cell通常是最佳选择，对于困难的扫描，没有突出的结构，建议使用较小的Cell而不是较大的Cell。平移估计通常比旋转更受到超大Cell的影响。当Cell较大时，运行时间通常稍微短一些(因此更少)，当单元格更小时，一些额外的时间花在初始化单元格上，但是执行实际优化时每次迭代的时间不依赖于单元格的大小。

6. Code And Result

基于PCL的NDT匹配算法示例代码如下

main.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
#include <iostream>
#include <thread>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;

int
main (int argc, char** argv)
{
    // Loading first scan of room.
    // 加载房间的第一帧扫描
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/msi/laser_scan_macher/3D_NDT_Matching_Pcl/room_scan1.pcd", *target_cloud) == -1)
    {
        PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;

    // Loading second scan of room from new perspective.
    // 加载从新视角得到的房间的第二帧扫描作为输入
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/msi/laser_scan_macher/3D_NDT_Matching_Pcl/room_scan2.pcd", *input_cloud) == -1)
    {
        PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;

    // Filtering input scan to roughly 10% of original size to increase speed of registration.
    // 将输入的扫描进行降采样滤波到原始尺寸的大概10%以提高匹配的速度。
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    // 设置八叉树网格大小
    approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
    approximate_voxel_filter.setInputCloud (input_cloud);
    approximate_voxel_filter.filter (*filtered_cloud);
    std::cout << "Filtered cloud contains " << filtered_cloud->size ()
              << " data points from room_scan2.pcd" << std::endl;

    // Initializing Normal Distributions Transform (NDT).
    // 初始化正态分布变换（NDT）
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

    // Setting scale dependent NDT parameters
    // Setting minimum transformation difference for termination condition.
    // 设置终止迭代的参数
    ndt.setTransformationEpsilon (0.01);

    // Setting maximum step size for More-Thuente line search.
    // 设置More-Thuente线搜索最大步长
    ndt.setStepSize (0.1);
    // Setting Resolution of NDT grid structure (VoxelGridCovariance).
    // 设置NDT网格结构的分辨率(尺寸)（VoxelGridCovariance）
    ndt.setResolution (1.0);

    // Setting max number of registration iterations.
    // 设置匹配迭代的最大次数
    ndt.setMaximumIterations (35);

    // Setting point cloud to be aligned.
    // 设置要配准的点云(也就是新的一帧扫描)
    ndt.setInputSource (filtered_cloud);
    // Setting point cloud to be aligned to.
    // 设置参考帧扫描，也就是目标
    ndt.setInputTarget (target_cloud);

    // Set initial alignment estimate found using robot odometry.
    // 使用里程计作为初始值
    Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

    // Calculating required rigid transform to align the input cloud to the target cloud.
    // 开始运算，进行匹配
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align (*output_cloud, init_guess);

    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
              << " score: " << ndt.getFitnessScore () << std::endl;

    // Transforming unfiltered, input cloud using found transform.
    // 使用优化得到的变换T，变换没有滤波的点云
    pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

    // Saving transformed input cloud.
    // 保存
    pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

    // Initializing point cloud visualizer
    pcl::visualization::PCLVisualizer::Ptr
            viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);

    // Coloring and visualizing target cloud (red).
    // 目标点云（参考帧）红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            target_color (target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "target cloud");

    // Coloring and visualizing transformed input cloud (green).
    // 输入点云，第二帧(绿色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            output_color (output_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");

    // Starting visualizer
    // 显示
    viewer_final->addCoordinateSystem (1.0, "global");
    viewer_final->initCameraParameters ();

    // Wait until visualizer window is closed.
    while (!viewer_final->wasStopped ())
    {
        viewer_final->spinOnce (100);
        std::this_thread::sleep_for(100ms);
    }

    return (0);
}

CMakeLists.txt

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
cmake_minimum_required(VERSION 2.8)

project(3D_NDT_Matching_Pcl)

set(CMAKE_CXX_STANDARD 14)

#PCL
find_package(PCL REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(${PROJECT_NAME} "main.cpp")
target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})

6.1. 结果

红色为参考帧，绿色为输入帧点云

6.1.1. 参数1 ， Cell分辨率为1m

可以看到，当Cell=1m的时候，对齐得比较好

6.1.2. 参数2 ， Cell尺寸 2m

粗看可发现基本对齐，但是有很多瑕疵的地方

细看边界，可以发现没有严格对齐

7. 扩展

  • Fixed discretisation
  • Octree discretisation
  • Iterative discretisation
  • Adaptive clustering
  • Linked cells
  • Trilinear interpolation
  • ...

8. 参考资料