slam系列1：open3d入门笔记

1. 读写数据

这里有很多测试用的pcd文件：
https://github.com/PointCloudLibrary/data/blob/master/tutorials/

import open3d as o3d
pcd = o3d.io.read_point_cloud("test.pcd")
o3d.io.write_point_cloud("write.pcd", pcd, True)	# 默认false，保存为Binarty；True 保存为ASICC形式
# o3d.visualization.draw_geometries([pcd]) # 传统画图法
o3d.visualization.draw_plotly_server([pcd]) # 在jupyter内展示，但是只能绘制点云

2. 常用工具

2.1 几何

# 读写
## 读取rgbd数据
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image,o3d.camera.PinholeCameraIntrinsic(
		o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
##

## 直接读取
pcd = o3d.io.read_point_cloud(...)

# 绘制法线
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(0.1, 30))     # 执行法线估计
o3d.visualization.draw_geometries([pcd], point_show_normal = True)

# 建立KDTree用于近邻查询
pcd_tree = o3d.geometry.KDTreeFlann(pcd)
pcd.paint_uniform_color([0.5, 0.5, 0.5])
# 使用K近邻，将第1500个点最近的5000个点设置为蓝色
[num_k, idx_k, _] = pcd_tree.search_knn_vector_3d(pcd.points[1500], 1000)    # 返回邻域点的个数和索引
np.asarray(pcd.colors)= [0, 0, 1]  # 进行赋色

# 建立八叉树用于空间划分
octree = o3d.geometry.Octree(max_depth=10)
octree.convert_from_point_cloud(pcd, size_expand=0.1)
o3d.visualization.draw_geometries([octree])

# 获取体素及下采样
voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcd, voxel_size=0.2)
downpcd = pcd.voxel_down_sample(0.5)


# compute_point_cloud_distance方法为源点云中的每个点计算到目标点云中最近点的距离。
dists = pcd1.compute_point_cloud_distance(pcd2)

# pcd.get_center() 获取质心
# pcd.select_by_index() 根据下标列表获取点云子集
# pcd.get_axis_aligned_bounding_box() 获取轴线方向最小包围盒
# pcd.get_oriented_bounding_box() 获取最小包围盒
# pcd.crop_point_cloud() 进行裁剪
# 计算点云凸包
hull, _ = pcd.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)

2.2 滤波

统计滤波

半径滤波

# 滤波例子
num_neighbors = 20 # K邻域点的个数
std_ratio = 2.0 # 标准差乘数
# 执行统计滤波，返回滤波后的点云sor_pcd和对应的索引ind
sor_pcd, ind = pcd.remove_statistical_outlier(num_neighbors, std_ratio)
sor_pcd.paint_uniform_color([0, 0, 1])
sor_noise_pcd = pcd.select_by_index(ind,invert = True)
sor_noise_pcd.paint_uniform_color([1, 0, 0])

2.3 聚类和分割

# 聚类例子
print("->正在DBSCAN聚类...")
eps =5    # 同一聚类中最大点间距
min_points = 10   # 有效聚类的最小点数
labels = np.array(pcd.cluster_dbscan(eps, min_points, print_progress=True))
max_label = labels.max()    # 获取聚类标签的最大值 [-1,0,1,2,...,max_label]，label = -1 为噪声，因此总聚类个数为 max_label + 1
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  # labels = -1 的簇为噪声，以黑色显示
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_plotly_server([pcd])

# 平面分割
print("->正在RANSAC平面分割...")
distance_threshold = 0.2    # 内点到平面模型的最大距离
ransac_n = 3                # 用于拟合平面的采样点数
num_iterations = 1000       # 最大迭代次数
# 返回模型系数plane_model和内点索引inliers，并赋值
plane_model, inliers = pcd.segment_plane(distance_threshold, ransac_n, num_iterations)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
# 平面内点点云
inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([0, 0, 1.0])
print(inlier_cloud)
# 平面外点点云
outlier_cloud = pcd.select_by_index(inliers, invert=True)
outlier_cloud.paint_uniform_color([1.0, 0, 0])
print(outlier_cloud)

# 隐藏点去除
print("->正在剔除隐藏点...")
diameter = np.linalg.norm(np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound()))
print("定义隐藏点去除的参数")
camera = [0, 0, diameter]       # 视点位置
radius = diameter * 100         # 噪声点云半径,The radius of the sperical projection
_, pt_map = pcd.hidden_point_removal(camera, radius)    # 获取视点位置能看到的所有点的索引 pt_map
# 可视点点云
pcd_visible = pcd.select_by_index(pt_map)
pcd_visible.paint_uniform_color([0, 0, 1])	# 可视点为蓝色
print("->可视点个数为：", pcd_visible)
# 隐藏点点云
pcd_hidden = pcd.select_by_index(pt_map, invert = True)
pcd_hidden.paint_uniform_color([1, 0, 0])	# 隐藏点为红色
print("->隐藏点个数为：", pcd_hidden)
print("->正在可视化可视点和隐藏点点云...")
o3d.visualization.draw_plotly_server([pcd_visible, pcd_hidden])

2.4 曲面重建

目前实现了如下几种：

mesh, densities  = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=8)
mesh  = o3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd,8)

我们对比一下效果

2.5 空间变换

# 平移
pcd.translate((tx,ty,tz),relative=True)
# 旋转
R1 = pcd.get_rotation_matrix_from_xyz((0,np.pi/2,0))
pcd.rotate(R1)   

# 点云缩放
pcd.scale(0.5,center=(1,1,1))	

# 一般变换：4×4变换矩阵，前3行3列为旋转矩阵，第4列前3行为平移向量，右下角为缩放系数。
pcd_scale2.transform(T)