『OPEN3D』1.1 点云处理
目录
1.open3d中的点云IO
2.点云的可视化
3 点云voxel下采样
4. 顶点法线估计
5.最小外界矩
6. 凸包计算
7. 点云距离计算
8. DBSCAN clustering聚类
9. RANSAC(Random Sample Consensus)
10. 点云平面分割
11. 隐藏点移除
12.outliers移除
13 最远点采样（Farthest Point Sample）
专栏地址：https://blog.csdn.net/qq_41366026/category_12186023.html
1.open3d中的点云IO
open3d.io.read_point_cloud(
filename,
format='auto',
remove_nan_points=False,
remove_infinite_points=False,
print_progress=False
)
"""函数返回值为open3d.geometry.PointCloud"""
open3d.io.write_point_cloud(filename,
pointcloud,
write_ascii=False,
compressed=False,
print_progress=False)
"""函数返回值为布尔类型"""
使用o3d中的data中的样例进行读取，如不能下载样例点云文件，可以自行更改读取文件地址
import open3d as o3d
if __name__ == "__main__":
pcd_data = o3d.data.PCDPointCloud()
print(
f"Reading pointcloud from file: fragment.pcd stored at {pcd_data.path}")
pcd = o3d.io.read_point_cloud(filename=pcd_data.path,
format="auto",
remove_nan_points=True,
remove_infinite_points=True,
print_progress=True
)
print(pcd)
print("Saving pointcloud to file: copy_of_fragment.pcd")
o3d.visualization.draw_geometries([pcd])
o3d.io.write_point_cloud("copy_of_fragment.pcd", pcd)
2.点云的可视化
这里会先介绍简单的点云可视化内容方便不了解open3d可视化的同学先简单的进行可视化操作，详细的可视化内容会在open3d可视化部分详细介绍
上例中使用open3d.visualization.draw_geometries 函数来简单可视化点云文件, 该函数有两个重载的版本：
版本1：
draw_geometries(geometry_list,
window_name="Open3D",
width=1920,
height=1080,
left=50,
top=50,
point_show_normal=False,
mesh_show_wireframe=False,
mesh_show_back_face=False)
"""函数返回值为空"""
| 参数名 | 说明 |
| geometry_list | 包含open3d.geometry所有类型的列表 |
| window_name | (str, optional, default='Open3D')，可视化窗口的显示标题 |
| width | (int, optional, default=1920)，可视化窗口的宽度 |
| height | (int, optional, default=1080)，可视化窗口的高度 |
| left | (int, optional, default=50)，可视化窗口的左边距 |
| top | (int, optional, default=50)，可视化窗口的顶部边距 |
| point_show_normal | (bool, optional, default=False)，如果设置为true，则可视化点法线，需要事先计算点云法线 |
| mesh_show_wireframe | (bool, optional, default=False)，如果设置为true，则可视化mesh面片的线条，mesh章节会用到该参数 |
| mesh_show_back_face | (bool, optional, default=False)，可视化mesh的背面，否则移动到物体内部向外看，则看不到mesh，mesh章节会用到该参数 |
版本2：
重载版本增加了相机的朝向、位置、姿态、缩放系数四个参数
draw_geometries(geometry_list,
window_name="Open3D",
width=1920,
height=1080,
left=50,
top=50,
point_show_normal=False,
mesh_show_wireframe=False,
mesh_show_back_face=False,
lookat,
up,
front,
zoom)
"""该函数返回值为空"""
| lookat | (numpy.ndarray[float64[3, 1]])，相机的注视方向向量 |
| up | (numpy.ndarray[float64[3, 1]])，相机的上方向向量 |
| front | (numpy.ndarray[float64[3, 1]])，相机的前矢量 |
| zoom | (float)，相机的缩放倍数 |
import open3d as o3d
if __name__ == "__main__":
pcd_data = o3d.data.PCDPointCloud()
print(
f"Reading pointcloud from file: fragment.pcd stored at {pcd_data.path}")
pcd = o3d.io.read_point_cloud(filename=pcd_data.path,
format="auto",
remove_nan_points=True,
remove_infinite_points=True,
print_progress=True
)
o3d.visualization.draw_geometries([pcd],
window_name = "可视化demo",
width = 1600,
height = 800,
left = 30,
top = 30,
point_show_normal = True)
按键N可以显示或关闭法向量显示
3 点云voxel下采样：
体素为三维空间中的一个矩形块，open3d中使用长宽高相等的体素块来填充三维空间，下采样使用常规体素栅格从输入点云创建均匀下采样的点云。它通常用作许多点云处理任务的预处理步骤。该算法分两步操作：
- 所有的点被分别装进与之对应的体素中。
- 每个被占用的体素通过平均内部的所有点来精确地生成一个点，用该点来代表这个体素。
体素可视化操作：
import numpy as np
import open3d as o3d
if __name__ == "__main__":
print("Downsample the point cloud with a voxel of 0.05")
pcd:o3d.geometry.PointCloud = o3d.io.read_point_cloud("fragment.pcd")
downpcd = pcd.voxel_down_sample(voxel_size=0.03)
pcd = pcd.translate([4,0,0])
o3d.visualization.draw_geometries([downpcd, pcd],
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
另一个voxel grid的例子：
左边是点云，右边是对点云进行voxel化后得到的体素模型
4. 顶点法线估计
estimate_normals计算每一个点的法向量，该函数寻找最近N个点并使用协方差分析来计算相邻点的法向量
estimate_normals(self, search_param=KDTreeSearchParamKNN with knn = 30, fast_normal_computation=True)
参数
| search_param | (open3d.geometry.KDTreeSearchParam, optional, default=KDTreeSearchParamKNN with knn = 30) ；KD树类用于最近邻搜索，radius指定了搜索的半径；max_nn指定了最多考虑多少个最进行进行拟合，节省计算时间 |
| fast_normal_computation | (bool, optional, default=True) ；若为真，法向量估计使用非迭代的方法从协方差矩阵中提取特征向量，提高计算速度，但会引入数值的不稳定 |
一种简单的法向量估计方法介绍：
确定曲面上某一点的法线的问题近似于估计与该点曲面相切的平面的法线问题，可以将该问题转成最小二乘平面拟合的估计问题。
对于选取的每个点Pi，先计算出协方差矩阵C,其中K为点Pi的最近邻的N个点，P拔为该点N个最近邻点的数值平均值；
lamda_j是协方差矩阵的第j个特征值，v_j是协方差矩阵的第j个特征向量。
注：协方差分析算法对于一个点产生两个方向的法向量且方向相反，在不知道物体的全局结构时，两个结果都可能是正确的，这是所谓的法线方向问题(normal orientation problem);在Open3D中，如果原始点云存在法线信息会尝试将法线的方向与原始点云法相方向匹配；否则将会随机选择一个方向。O3D中有如下函数可以调整法线方向：
orient_normals_consistent_tangent_plane(self, k) |
法线方向对其连续的切平面，K为最近邻点的个数 |
|
法线方向以orientation_reference的为相对方向进行设定 |
|
法线方向朝相机位置进行设定 |
import numpy as np
import open3d as o3d
if __name__ == "__main__":
print("Downsample the point cloud with a voxel of 0.05")
pcd:o3d.geometry.PointCloud = o3d.io.read_point_cloud("/home/nathan/open3d_data/extract/PCDPointCloud/fragment.pcd")
downpcd:o3d.geometry.PointCloud = pcd.voxel_down_sample(voxel_size=0.03)
pcd = pcd.translate([4,0,0])
print("Recompute the normal of the downsampled point cloud")
downpcd.estimate_normals(
search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
o3d.visualization.draw_geometries([downpcd, pcd],
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
注：±可以调整法线的长度。
在估计完点云的法向量后，可以直接访问open3d.geometry.PointCloud中的normal与索引访问每个点云的法向量信息。
print(downpcd.normals[0])
Print a normal vector of the 0th point [-0.27566603 -0.89197839 -0.35830543]
5.最小外界矩
在open3d终支持两种最小外接矩的计算，分别是按照坐标轴生成get_axis_aligned_bounding_box
和基于物体方向生成的get_oriented_bounding_box。
代码如下
import open3d as o3d
if __name__ == "__main__":
sample_ply_data = o3d.data.PLYPointCloud()
pcd = o3d.io.read_point_cloud(sample_ply_data.path)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
print(pcd)
axis_aligned_bounding_box = pcd.get_axis_aligned_bounding_box()
axis_aligned_bounding_box.color = (1, 0, 0)
oriented_bounding_box = pcd.get_oriented_bounding_box()
oriented_bounding_box.color = (0, 1, 0)
coor_mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(1)
print(
"Displaying axis_aligned_bounding_box in red and oriented bounding box in green ..."
)
o3d.visualization.draw_geometries(
[pcd, coor_mesh,axis_aligned_bounding_box, oriented_bounding_box])
红色box为基于坐标轴生成的box，与坐标轴平齐；绿色是基于物体方向生成的box，方向与物体方向平齐
6. 凸包计算
凸包（convex hull）计算，可以得到电晕的最小包裹平面，在open3d终使用compute_convex_hull 方法可以计算一个物体的凸包，其实现方法是基于Qhull。
凸包计算案例：
import open3d as o3d
if __name__ == "__main__":
print("Displaying pointcloud with convex hull ...")
bunny = o3d.data.BunnyMesh()
mesh:o3d.geometry.TriangleMesh = o3d.io.read_triangle_mesh(bunny.path)
mesh.paint_uniform_color([0,0,1])
pcl = mesh.sample_points_poisson_disk(number_of_points=10000)
hull, _ = pcl.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
hull_ls.paint_uniform_color((1, 0, 0))
o3d.visualization.draw_geometries([pcl, hull_ls])
注：
1.sample_points_poisson_disk在TriangleMesh类中，用于对mesh进行采样得到点云，且每个点到相邻点之间的间距大致相等，该方法基于 “Sample Elimination for Generating Poisson Disk Sample Sets”, EUROGRAPHICS, 2015. 返回结果为open3d.geometry.PointCloud
2.sample_points_uniformly 均匀的在mesh中进行采样得到点云
7. 点云距离计算
在open3d中，提供了compute_point_cloud_distance函数用于计算原点云中每个点与目标点云中所有点中距离最短的点的之间的距离。
提供了compute_nearest_neighbor_distance函数用于计算自身点云中每个点距离自身点云中最近点的距离。
样例
import open3d as o3d
import numpy as np
if __name__ == '__main__':
l1 = np.array([[i,i,i] for i in range(1,5)])
pcd1 = o3d.geometry.PointCloud()
pcd1.points=o3d.utility.Vector3dVector(l1)
pcd1.paint_uniform_color([1,0,0])
l2 = np.array([[i*3,0,0] for i in range(1,3)])
pcd2 = o3d.geometry.PointCloud()
pcd2.points=o3d.utility.Vector3dVector(l2)
pcd2.paint_uniform_color([0,1,0])
coor_mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1)
dists = pcd1.compute_point_cloud_distance(pcd2)
dists = np.asarray(dists)
print(dists)
o3d.visualization.draw_geometries([pcd1, pcd2,coor_mesh])
计算结果得到pcd1中每个点云到pcd2中每个点云的最小距离
8. DBSCAN clustering聚类
对于一个给定的点云，我们如果要将不同的物体分开，可以使用聚类的方法。在open3d中提供了DBSCAN聚类 （Ester and H.-P. Kriegel and J Sander and X. Xu, A density-based algorithm for discovering clusters in large spatial databases with noise, KDD, 1996.），该聚类算法为基于密度的聚类方法，提供函数接口cluster_dbscan。cluster_dbscan函数需要两个参数，eps和min_points；eps定义了一个cluster中相邻点之间的距离，min_points定义了最少需要多少个点才可以算为一个cluster。
注：DBSCAN算法预先计算了每个点在eps范围内的所有邻居点，因此在eps较大时，该算法将会消耗大量的内存
import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt
if __name__ == "__main__":
sample_ply_data = o3d.data.PLYPointCloud()
pcd = o3d.io.read_point_cloud(sample_ply_data.path)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
with o3d.utility.VerbosityContextManager(
o3d.utility.VerbosityLevel.Debug) as cm:
"""
label为每个点云对应的类别，
其中label为-1的点云被认为是噪音
"""
labels = np.array(
pcd.cluster_dbscan(eps=0.02, min_points=10, print_progress=True))
max_label = labels.max()
print(f"point cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
colors[labels < 0] = 0
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd])
注：open3d中使用open3d.utility.VerbosityContextManager来管理输出的日志等级，分别有如下几种
| Debug | |
| Error | |
| Info | |
| Warning |
9. RANSAC(Random Sample Consensus)
此处先介绍随即采样一致性（RANSAC）后再介绍基于模型的点云分割，RANSAC是一种使用迭代方法从一个包含离群点（outliers）中估计数学模型参数的方法。该算法由Fischler and Bolles在1981年发表。RANSAC假设所有的数据仅有内点（inliers）和外点（outliers）组成，内点可以通过模型的参数求得，外点则不适应该模型。简而言之，RANSAC对于一个给定的模型可以从杂乱的数据中估计出最符合当前数据的模型参数。
RANSAC求取步骤：
输入ransac算法的是被观测的数据集和一个可以解释该数据的参数化模型以及算法迭代的置信度参数；然后通过迭代的方式随机选择原始数据中的一个子集数据作为假想的inliers，并通过这些inliers求解模型参数；再通过以下步骤进行测试：
- 使用假想的内点（inliers）来求解模型的所有参数
- 使用原始数据中所有的数据来测试求解的模型参数，如果某一个数据与本次模型参数匹配，该数据也被认为是假想的内点
- 如果再所有数据测试中，有足够多的数据被认为是假想的内点，那么可以认为本子集数据估计得到的模型参数是不错的
- 使用2中所有被认为是内点的所有数据，再次估计模型参数来得到更加泛化的结果
- 最后，使用重新估计得到的模型从所有内点中评估计算得到该模型的误差
上述过程会执行指定的迭代次数，每次都会产生一个模型，如果本次太少的内点与该模型匹配，本次的估计结果会被丢弃；否则通过4步骤来优化模型后再使用5步骤计算该模型的相对误差。每次都会将相对上一次误差更小的模型保存。
如果熟悉线性代数的话，该问题是一个超定方程组的求解问题，可以使用最小二乘或其衍生的svd来进行求解；但是对于计算机视觉中，无论是点云或者是图像的关键点，数据量都是巨大的，对巨大的数据进行矩阵分解耗时和计算量都是无法承受的。同时在拥有大量噪音的数据中，ransac可以比类最小二乘的方法更加健壮。但是ransac由于迭代次数是没有上限的，只要迭代的次数足够大，它总能求得最好的结果；如果迭代次数不够，求取的结果不能达到最优；因此ransac在运用时需要设置迭代次数与特定模型需要的置信度门槛。
对于能用ransac求解的问题必须是一个可以用一个数学模型表达的式子，如果一组数据中包含多个数学模型，那么ransac就只能找到其中一个。
下图展示了一个ransac求取线性回归模型的应用，左图是原始数据，右图中蓝点是求取得到的inliers，红是是outliers。
python实现的ransac线性回归代码：
import numpy as np
from matplotlib import pyplot as plt
import random
if __name__ == '__main__':
SIZE = 500
OUT = 230
X = np.linspace(0, 100, SIZE)
Y = []
for i in X:
if random.randint(0, 10) > 5:
Y.append(random.randint(0, OUT))
else:
if random.randint(0, 10) > 5:
Y.append(3 * i + 10 + 3 * random.random())
else:
Y.append(3 * i + 10 - 3 * random.random())
X_data = np.array(X)
Y_data = np.array(Y)
plt.scatter(X_data, Y_data)
plt.show()
iters = 10000
epsilon = 3
threshold = (SIZE - OUT) / SIZE + 0.01
best_a, best_b = 0, 0
pre_total = 0
for i in range(iters):
sample_index = random.sample(range(SIZE), 2)
x_1 = X_data[sample_index[0]]
x_2 = X_data[sample_index[1]]
y_1 = Y_data[sample_index[0]]
y_2 = Y_data[sample_index[1]]
a = (y_2 - y_1) / (x_2 - x_1)
b = y_1 - a * x_1
total_in = 0
for index in range(SIZE):
y_estimate = a * X_data[index] + b
if abs(y_estimate - Y_data[index]) < epsilon:
total_in += 1
if total_in > pre_total:
pre_total = total_in
best_a = a
best_b = b
if total_in > SIZE * threshold:
break
print("迭代{}次,a = {}, b = {}".format(i, best_a, best_b))
x_line = X_data
y_line = best_a * x_line + best_b
plt.plot(x_line, y_line, c='r')
plt.scatter(X_data, Y_data)
plt.show()
10. 点云平面分割
Open3D支持使用RANSAC对点云进行分割，使用segment_plane可以完成点云中最大平面的分割，该方法有三个参数
distance_threshold |
点到平面模型的最大距离，小于或等于该距离的点被认为是inliers |
ransac_n |
每次迭代中，选取多少个点作为子集求解模型参数 |
num_iterations |
迭代次数 |
示例
import open3d as o3d
if __name__ == "__main__":
sample_pcd_data = o3d.data.PCDPointCloud()
pcd = o3d.io.read_point_cloud(sample_pcd_data.path)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
ransac_n=3,
num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
print("Displaying pointcloud with planar points in red ...")
"""
select_by_index 根据索引在原始点云中得到inlier_cloud，
如果设置invert=True则反转选择结果为outlier_cloud
"""
inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud = pcd.select_by_index(inliers, invert=True)
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
得到该平面模型的数学模型为：0.06x + -0.10y + 0.99z + 1.06 = 0
11. 隐藏点移除
隐藏点移除用于移除当前视角看不到的点，在open3d中实现了_Katz and A. Tal and R. Basri, Direct visibility of point sets, SIGGRAPH, 2007._ 的给定视角下无需表面重建或法向量的方式完成可见点的估计，其API为hidden_point_removal(self, camera_location, radius)；
| camera_location | (numpy.ndarray[numpy.float64[3, 1]])；相机的位置，所有从该方位看不到的点都会被移除 |
| radius | 球面投影的半径 |
示例：
import open3d as o3d
import numpy as np
if __name__ == "__main__":
armadillo_data = o3d.data.ArmadilloMesh()
pcd = o3d.io.read_triangle_mesh(
armadillo_data.path).sample_points_poisson_disk(5000)
diameter = np.linalg.norm(
np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound()))
o3d.visualization.draw_geometries([pcd])
camera = [0, 0, diameter]
radius = diameter * 100
_, pt_map = pcd.hidden_point_removal(camera, radius)
print("Displaying point cloud after hidden point removal ...")
pcd1 = pcd.select_by_index(pt_map)
pcd1.translate([160,0,0])
o3d.visualization.draw_geometries([pcd1, pcd])
12.outliers移除
从深度相机或其他扫描设备获取的点云数据，会包含噪音数据，因此open3d中实现了两种outliers移除的方式，分别为：
1.Statistical outlier removal，该方法移除那些相比于他相邻点平均距离更远的点，其包含两个参数；nb_neighbors指定使用多少个相邻点来计算给定点的相邻平均距离，std_ratio设置了该点允许再平均距离上的标准偏差，该数值越低，算法会更加激进。
import open3d as o3d
import numpy as np
def display_inlier_outlier(cloud, ind):
inlier_cloud = cloud.select_by_index(ind)
outlier_cloud = cloud.select_by_index(ind, invert=True)
print("Showing outliers (red) and inliers (gray): ")
"""
给属于outliers的点云上色，使用paint_uniform_color，
在Open3D中，颜色顺序为RGB，颜色范围为0-1
"""
outlier_cloud.paint_uniform_color([1, 0, 0])
inlier_cloud.paint_uniform_color([0.8, 0.8, 0.8])
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
if __name__ == "__main__":
ptcloud_data = o3d.data.PLYPointCloud()
print("Load a ply point cloud, print it, and render it")
pcd = o3d.io.read_point_cloud(ptcloud_data.path)
R = pcd.get_rotation_matrix_from_xyz((np.pi, 0, 0))
pcd.rotate(R, center=(0, 0, 0))
o3d.visualization.draw([pcd])
print("Downsample the point cloud with a voxel of 0.02")
voxel_down_pcd = pcd.voxel_down_sample(voxel_size=0.02)
o3d.visualization.draw([voxel_down_pcd])
print("Statistical oulier removal")
cl, ind = voxel_down_pcd.remove_statistical_outlier(nb_neighbors=20,
std_ratio=2.0)
display_inlier_outlier(voxel_down_pcd, ind)
2.Radius outlier removal，该方法移除在给定球形空间中，邻居过少的点，包含参数nb_points和radius；nb_points指定了该球形空间中最少需要包含的点个数，radius指定了该球形空间的半径大小
import open3d as o3d
import numpy as np
def display_inlier_outlier(cloud, ind):
inlier_cloud = cloud.select_by_index(ind)
outlier_cloud = cloud.select_by_index(ind, invert=True)
print("Showing outliers (red) and inliers (gray): ")
"""
给属于outliers的点云上色，使用paint_uniform_color，
在Open3D中，颜色顺序为RGB，颜色范围为0-1
"""
outlier_cloud.paint_uniform_color([1, 0, 0])
inlier_cloud.paint_uniform_color([0.8, 0.8, 0.8])
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
if __name__ == "__main__":
ptcloud_data = o3d.data.PLYPointCloud()
print("Load a ply point cloud, print it, and render it")
pcd = o3d.io.read_point_cloud(ptcloud_data.path)
R = pcd.get_rotation_matrix_from_xyz((np.pi, 0, 0))
pcd.rotate(R, center=(0, 0, 0))
o3d.visualization.draw([pcd])
print("Downsample the point cloud with a voxel of 0.02")
voxel_down_pcd = pcd.voxel_down_sample(voxel_size=0.02)
o3d.visualization.draw([voxel_down_pcd])
print("Radius oulier removal")
cl, ind = voxel_down_pcd.remove_radius_outlier(nb_points=16, radius=0.05)
display_inlier_outlier(voxel_down_pcd, ind)
其中红色的点被认为是outliers，可以过滤掉
13 最远点采样（Farthest Point Sample）
最远点采样（FPS）用于在密度不等的点云中，可以按照空间距离来均匀的对点云数据进行采样，在点云的模式识别中广泛应用，PointNet++等点云网络中均先对原始点云数据进行FPS之后，将FPS之后的结果再送入网络模型进行操作。常规的随机采样不能很好的对密度不均匀的数据进行采样，会导致采样点不均匀的情况。
import open3d as o3d
if __name__ == "__main__":
bunny = o3d.data.BunnyMesh()
pcd = o3d.io.read_point_cloud(bunny.path)
pcd.paint_uniform_color([0.5, 0.5, 0.5])
pcd_down = pcd.farthest_point_down_sample(1000)
pcd_down.paint_uniform_color([0, 1, 0])
o3d.visualization.draw_geometries([pcd, pcd_down])
下图中，绿色点为FPS算法采样后得到的点
