点云处理（基于Open3D）

1. 基本概念

• 点云的用途：
  • 点云数据能够以较小的存储成本获得物体准确的拓扑结构和几何结构，因而获得越来越广泛的关注。
  • 在实际的采集过程中，因为被测物体尺寸过大，物体表面被遮挡或者三维扫描设备的扫描角度等因素，单次的扫描往往得不到物体完整的几何信息。
  • 为了获得被测物体的完整几何信息，就需要将不同视角即不同参考坐标下的两组或者多组点云统一到统一坐标系下，进行点云的配准。

1.1 点云格式 (Point Cloud Format)

Format	Description
xyz	Each line contains [x, y, z], where x, y, z are the 3D coordinates
xyzn	Each line contains [x, y, z, nx, ny, nz], where nx, ny, nz are the normals
xyzrgb	Each line contains [x, y, z, r, g, b], where r, g, b are in floats of range [0, 1]
pts	The first line is an integer representing the number of points. Each subsequent line contains [x, y, z, i, r, g, b], where r, g, b are in uint8
ply	See Polygon File Format, the ply file can contain both point cloud and mesh data
pcd	See Point Cloud Data

• 文件格式及开发者

Format	Developer
PLY	a polygon file format, developed at Stanford University by Turk et al
STL	a file format native to the stereolithography CAD software created by 3D Systems
OBJ	a geometry definition file format first developed by Wavefront Technologies
X3D	the ISO standard XML-based file format for representing 3D computer graphics data
PCD	Point Cloud Library (PCL)

1.1.1 PLY (多边形集合）

• PLY：Polygon File Format
• PLY文件：由多边形集合组成的图形对象（Graphical Objects）
• 两种存储模式
  • ASCII模式
  • Binary模式
• 典型的文件结构
  • 组成元素：顶点列表、面列表

  Header
  Vertex List
  Face List
  (lists of other elements)

• 立方体描述1

ply
format ascii 1.0           { ascii/binary, format version number }
comment made by Greg Turk  { comments keyword specified, like all lines }
comment this file is a cube
element vertex 8           { define "vertex" element, 8 of them in file }
property float x           { vertex contains float "x" coordinate }
property float y           { y coordinate is also a vertex property }
property float z           { z coordinate, too }
element face 6             { there are 6 "face" elements in the file }
property list uchar int vertex_index { "vertex_indices" is a list of ints }
end_header                 { delimits the end of the header }
0 0 0                      { start of vertex list }
0 0 1
0 1 1
0 1 0
1 0 0
1 0 1
1 1 1
1 1 0
4 0 1 2 3                  { start of face list }
4 7 6 5 4
4 0 4 5 1
4 1 5 6 2
4 2 6 7 3
4 3 7 4 0

• 立方体描述2

ply
format ascii 1.0
comment author: Greg Turk
comment object: another cube
element vertex 8
property float x
property float y
property float z
property uchar red                   { start of vertex color }
property uchar green
property uchar blue
element face 7
property list uchar int vertex_index  { number of vertices for each face }
element edge 5                        { five edges in object }
property int vertex1                  { index to first vertex of edge }
property int vertex2                  { index to second vertex }
property uchar red                    { start of edge color }
property uchar green
property uchar blue
end_header
0 0 0 255 0 0                         { start of vertex list }
0 0 1 255 0 0
0 1 1 255 0 0
0 1 0 255 0 0
1 0 0 0 0 255
1 0 1 0 0 255
1 1 1 0 0 255
1 1 0 0 0 255
3 0 1 2                           { start of face list, begin with a triangle }
3 0 2 3                           { another triangle }
4 7 6 5 4                         { now some quadrilaterals }
4 0 4 5 1
4 1 5 6 2
4 2 6 7 3
4 3 7 4 0
0 1 255 255 255                   { start of edge list, begin with white edge }
1 2 255 255 255
2 3 255 255 255
3 0 255 255 255
2 0 0 0 0                         { end with a single black line }

1.1.2 PCD (Point Cloud Data)

• 实例

# .PCD v.7 - Point Cloud Data file format
VERSION .7  # 版本号 PCV_V7
FIELDS x y z rgb  # 每个点包含的数据
SIZE 4 4 4 4    # 每个域的字节数
TYPE F F F F    # 每个域的数据类型
COUNT 1 1 1 1   # 每个域的元素个数 
WIDTH 213       # 点云集中点的个数
HEIGHT 1        # 表示为无组织的数据集
VIEWPOINT 0 0 0 1 0 0 0
POINTS 213
DATA ascii
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06
0.81915 0.32 0 4.2108e+06
0.97192 0.278 0 4.2108e+06
0.944 0.29474 0 4.2108e+06
0.98111 0.24247 0 4.2108e+06
0.93655 0.26143 0 4.2108e+06
0.91631 0.27442 0 4.2108e+06
0.81921 0.29315 0 4.2108e+06
0.90701 0.24109 0 4.2108e+06
0.83239 0.23398 0 4.2108e+06
0.99185 0.2116 0 4.2108e+06
0.89264 0.21174 0 4.2108e+06
0.85082 0.21212 0 4.2108e+06
0.81044 0.32222 0 4.2108e+06
0.74459 0.32192 0 4.2108e+06
0.69927 0.32278 0 4.2108e+06
0.8102 0.29315 0 4.2108e+06
0.75504 0.29765 0 4.2108e+06
0.8102 0.24399 0 4.2108e+06
0.74995 0.24723 0 4.2108e+06
0.68049 0.29768 0 4.2108e+06
0.66509 0.29002 0 4.2108e+06
0.69441 0.2526 0 4.2108e+06
0.62807 0.22187 0 4.2108e+06
0.58706 0.32199 0 4.2108e+06
0.52125 0.31955 0 4.2108e+06
0.49351 0.32282 0 4.2108e+06
0.44313 0.32169 0 4.2108e+06
0.58678 0.2929 0 4.2108e+06
0.53436 0.29164 0 4.2108e+06
0.59308 0.24134 0 4.2108e+06
0.5357 0.2444 0 4.2108e+06
0.50043 0.31235 0 4.2108e+06
0.44107 0.29711 0 4.2108e+06
0.50727 0.22193 0 4.2108e+06
0.43957 0.23976 0 4.2108e+06
0.8105 0.21112 0 4.2108e+06
0.73555 0.2114 0 4.2108e+06
0.69907 0.21082 0 4.2108e+06
0.63327 0.21154 0 4.2108e+06
0.59165 0.21201 0 4.2108e+06
0.52477 0.21491 0 4.2108e+06
0.49375 0.21006 0 4.2108e+06
0.4384 0.19632 0 4.2108e+06
0.43425 0.16052 0 4.2108e+06
0.3787 0.32173 0 4.2108e+06
0.33444 0.3216 0 4.2108e+06
0.23815 0.32199 0 4.808e+06
0.3788 0.29315 0 4.2108e+06
0.33058 0.31073 0 4.2108e+06
0.3788 0.24399 0 4.2108e+06
0.30249 0.29189 0 4.2108e+06
0.23492 0.29446 0 4.808e+06
0.29465 0.24399 0 4.2108e+06
0.23514 0.24172 0 4.808e+06
0.18836 0.32277 0 4.808e+06
0.15992 0.32176 0 4.808e+06
0.08642 0.32181 0 4.808e+06
0.039994 0.32283 0 4.808e+06
0.20039 0.31211 0 4.808e+06
0.1417 0.29506 0 4.808e+06
0.20921 0.22332 0 4.808e+06
0.13884 0.24227 0 4.808e+06
0.085123 0.29441 0 4.808e+06
0.048446 0.31279 0 4.808e+06
0.086957 0.24399 0 4.808e+06
0.3788 0.21189 0 4.2108e+06
0.29465 0.19323 0 4.2108e+06
0.23755 0.19348 0 4.808e+06
0.29463 0.16054 0 4.2108e+06
0.23776 0.16054 0 4.808e+06
0.19016 0.21038 0 4.808e+06
0.15704 0.21245 0 4.808e+06
0.08678 0.21169 0 4.808e+06
0.012746 0.32168 0 4.808e+06
-0.075715 0.32095 0 4.808e+06
-0.10622 0.32304 0 4.808e+06
-0.16391 0.32118 0 4.808e+06
0.00088411 0.29487 0 4.808e+06
-0.057568 0.29457 0 4.808e+06
-0.0034333 0.24399 0 4.808e+06
-0.055185 0.24185 0 4.808e+06
-0.10983 0.31352 0 4.808e+06
-0.15082 0.29453 0 4.808e+06
-0.11534 0.22049 0 4.808e+06
-0.15155 0.24381 0 4.808e+06
-0.1912 0.32173 0 4.808e+06
-0.281 0.3185 0 4.808e+06
-0.30791 0.32307 0 4.808e+06
-0.33854 0.32148 0 4.808e+06
-0.21248 0.29805 0 4.808e+06
-0.26372 0.29905 0 4.808e+06
-0.22562 0.24399 0 4.808e+06
-0.25035 0.2371 0 4.808e+06
-0.29941 0.31191 0 4.808e+06
-0.35845 0.2954 0 4.808e+06
-0.29231 0.22236 0 4.808e+06
-0.36101 0.24172 0 4.808e+06
-0.0034393 0.21129 0 4.808e+06
-0.07306 0.21304 0 4.808e+06
-0.10579 0.2099 0 4.808e+06
-0.13642 0.21411 0 4.808e+06
-0.22562 0.19323 0 4.808e+06
-0.24439 0.19799 0 4.808e+06
-0.22591 0.16041 0 4.808e+06
-0.23466 0.16082 0 4.808e+06
-0.3077 0.20998 0 4.808e+06
-0.3413 0.21239 0 4.808e+06
-0.40551 0.32178 0 4.2108e+06
-0.50568 0.3218 0 4.2108e+06
-0.41732 0.30844 0 4.2108e+06
-0.44237 0.28859 0 4.2108e+06
-0.41591 0.22004 0 4.2108e+06
-0.44803 0.24236 0 4.2108e+06
-0.50623 0.29315 0 4.2108e+06
-0.50916 0.24296 0 4.2108e+06
-0.57019 0.22334 0 4.2108e+06
-0.59611 0.32199 0 4.2108e+06
-0.65104 0.32199 0 4.2108e+06
-0.72566 0.32129 0 4.2108e+06
-0.75538 0.32301 0 4.2108e+06
-0.59653 0.29315 0 4.2108e+06
-0.65063 0.29315 0 4.2108e+06
-0.59478 0.24245 0 4.2108e+06
-0.65063 0.24399 0 4.2108e+06
-0.70618 0.29525 0 4.2108e+06
-0.76203 0.31284 0 4.2108e+06
-0.70302 0.24183 0 4.2108e+06
-0.77062 0.22133 0 4.2108e+06
-0.41545 0.21099 0 4.2108e+06
-0.45004 0.19812 0 4.2108e+06
-0.4475 0.1673 0 4.2108e+06
-0.52031 0.21236 0 4.2108e+06
-0.55182 0.21045 0 4.2108e+06
-0.5965 0.21131 0 4.2108e+06
-0.65064 0.2113 0 4.2108e+06
-0.72216 0.21286 0 4.2108e+06
-0.7556 0.20987 0 4.2108e+06
-0.78343 0.31973 0 4.2108e+06
-0.87572 0.32111 0 4.2108e+06
-0.90519 0.32263 0 4.2108e+06
-0.95526 0.34127 0 4.2108e+06
-0.79774 0.29271 0 4.2108e+06
-0.85618 0.29497 0 4.2108e+06
-0.79975 0.24326 0 4.2108e+06
-0.8521 0.24246 0 4.2108e+06
-0.91157 0.31224 0 4.2108e+06
-0.95031 0.29572 0 4.2108e+06
-0.92223 0.2213 0 4.2108e+06
-0.94979 0.24354 0 4.2108e+06
-0.78641 0.21505 0 4.2108e+06
-0.87094 0.21237 0 4.2108e+06
-0.90637 0.20934 0 4.2108e+06
-0.93777 0.21481 0 4.2108e+06
0.22244 -0.0296 0 4.808e+06
0.2704 -0.078167 0 4.808e+06
0.24416 -0.056883 0 4.808e+06
0.27311 -0.10653 0 4.808e+06
0.26172 -0.10653 0 4.808e+06
0.2704 -0.1349 0 4.808e+06
0.24428 -0.15599 0 4.808e+06
0.19017 -0.025297 0 4.808e+06
0.14248 -0.02428 0 4.808e+06
0.19815 -0.037432 0 4.808e+06
0.14248 -0.03515 0 4.808e+06
0.093313 -0.02428 0 4.808e+06
0.044144 -0.02428 0 4.808e+06
0.093313 -0.03515 0 4.808e+06
0.044144 -0.03515 0 4.808e+06
0.21156 -0.17357 0 4.808e+06
0.029114 -0.12594 0 4.2108e+06
0.036583 -0.15619 0 4.2108e+06
0.22446 -0.20514 0 4.808e+06
0.2208 -0.2369 0 4.808e+06
0.2129 -0.208 0 4.808e+06
0.19316 -0.25672 0 4.808e+06
0.14497 -0.27484 0 4.808e+06
0.030167 -0.18748 0 4.2108e+06
0.1021 -0.27453 0 4.808e+06
0.1689 -0.2831 0 4.808e+06
0.13875 -0.28647 0 4.808e+06
0.086993 -0.29568 0 4.808e+06
0.044924 -0.3154 0 4.808e+06
-0.0066125 -0.02428 0 4.808e+06
-0.057362 -0.02428 0 4.808e+06
-0.0066125 -0.03515 0 4.808e+06
-0.057362 -0.03515 0 4.808e+06
-0.10653 -0.02428 0 4.808e+06
-0.15266 -0.025282 0 4.808e+06
-0.10653 -0.03515 0 4.808e+06
-0.16036 -0.037257 0 4.808e+06
0.0083286 -0.1259 0 4.2108e+06
0.0007442 -0.15603 0 4.2108e+06
-0.1741 -0.17381 0 4.808e+06
-0.18502 -0.02954 0 4.808e+06
-0.20707 -0.056403 0 4.808e+06
-0.23348 -0.07764 0 4.808e+06
-0.2244 -0.10653 0 4.808e+06
-0.23604 -0.10652 0 4.808e+06
-0.20734 -0.15641 0 4.808e+06
-0.23348 -0.13542 0 4.808e+06
0.0061083 -0.18729 0 4.2108e+06
-0.066235 -0.27472 0 4.808e+06
-0.17577 -0.20789 0 4.808e+06
-0.10861 -0.27494 0 4.808e+06
-0.15584 -0.25716 0 4.808e+06
-0.0075775 -0.31546 0 4.808e+06
-0.050817 -0.29595 0 4.808e+06
-0.10306 -0.28653 0 4.808e+06
-0.1319 -0.2831 0 4.808e+06
-0.18716 -0.20571 0 4.808e+06
-0.18369 -0.23729 0 4.808e+06

2. RANSAC

2.1 简介

• RANSAC：RANdom SAmple Consensus（随机一致性采样）
• 核心思想：
  • 随机性：根据正确数据出现概率去随机选取抽样数据，根据大数定律，随机性模拟可以近似得到正确结果
  • 假设性：假设选取出的抽样数据都是正确数据，然后用这些正确数据回归模型，去计算其他点，然后对这次结果进行一个评分
• 用途：
  • 直线拟合
  • 平面拟合
  • 计算图像或点云间的变换矩阵
  • 计算基础矩阵
• 工作原理：
  • 采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数
  • RANSAC算法假设数据中包含正确数据和异常数据（或称为噪声）
  • 正确数据记为内点（inliers），异常数据记为外点（outliers）
  • 同时RANSAC也假设，给定一组正确的数据，存在可以计算出符合这些数据的模型参数的方法
• 基本假设：
  • 数据由“局内点”组成，例如：数据的分布可以用一些模型参数来解释
  • “局外点”是不能适应该模型的数据
  • 除此之外的数据属于噪声
• 基本流程
  • 通过反复选择数据中的一组随机子集来达成目标
  • 被选取的子集被假设为局内点，并用下述方法进行验证：
    • 1.有一个模型适用于假设的局内点，即所有的未知参数都能从假设的局内点计算得出
    • 2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点
    • 3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理
    • 4.然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过
    • 5.最后，通过估计局内点与模型的错误率来评估模型
  • 这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为它比现有的模型更好而被选用
• 示例
  

2.2 应用

2.2.1 直线拟合

（1）要得到一个直线模型，需要两个点唯一确定一个直线方程。所以第一步随机选择两个点。
（2）通过这两个点，可以计算出这两个点所表示的模型方程y=ax+b。
（3）将所有的数据点套到这个模型中计算误差。
（4）找到所有满足误差阈值的点。
（5）然后我们再重复（1）~（4）这个过程，直到达到一定迭代次数后，选出那个被支持的最多的模型，作为问题的解。如下图所示：

• 如上图所示:可以发现，虽然这个数据集中外点和内点的比例几乎相等，但是RANSAC算法还是能找到最合适的解。
• 这个问题如果使用最小二乘法进行优化，由于噪声数据的干扰，我们得到的结果肯定是一个错误的结果，如下图所示。
  
• 这是由于最小二乘法是一个将外点参与讨论的代价优化问题，而RANSAC是一个使用内点进行优化的问题。经实验验证，对于包含80%误差的数据集，RANSAC的效果远优于直接的最小二乘法。

3. ICP

• ICP：迭代最近点(Iterative Closest Points, ICP)
• 用途：
  • ICP是一种点集对点集的配准方法
  • 它计算如何把一个数据集中的点云经过平移和旋转，使其与另一个数据集中的点云重合
  • 点集与点集之间的配准
  • 点集到模型的配准
  • 模型到模型的配准
• ICP输入：两个点云
• ICP输出：(R+T)
  • 两个点云间的旋转量（R）
  • 两个点云间的平移量（T）

3.1 求解步骤

• 点云数据集：
  • $B$：基准数据点集
  • $P$：待配准点集
• 求解步骤
  • 搜索最近点：
    • 取$P$中一点$P_i$，在$B$中找出距离$P_i$最近的$B_i$，则$(P_i,B_i)$就构成了一组对应点对集，但是$P_i$与$B_i$之间存在着旋转和平移关系$(R,T)$
    • 两个点之间存在一个确定的$R$和$T$，但两个点云间的$R$和$T$必须尽量满足所有匹配的点对集
  • 求解变换关系$(R,T)$
    • n对点$(B_i,B_i)$生成$n$个方程组，那么可运用数学方法求解得出$(R,T)$ ，但是为了求解更加精确的变换关系，采用了迭代算法
  • 应用变换
    • 对点集$P$中的每一个点$p_i$运用变换关系得到点集$P^{\prime }$，定义函数$E$：
      $$E_d(R,T)=\frac{1}{n}\sum _{i=1}^n(R\cdot P_i+T-B_i{)}^2$$
    • 根据精度要求，定义终止迭代的条件，即$E$小于一个具体值时终止迭代
  • 重复迭代
    • 如果某次迭代满足要求，则终止迭代，输出最优$(R,T)$，否则继续迭代
    • 注意：在每一次迭代开始时都要重新寻找对应点集。也就是说要把结果变换的$P^{\prime }$带入函数$E$中继续迭代

参考

https://blog.csdn.net/pi9nc/article/details/26596519
https://zhuanlan.zhihu.com/p/45532306