本文主要记录pcl点云处理常用工具，在此将每种方法写为一个函数，便于使用的时候直接调用。
本文主要记录pcl点云处理常用工具，在此将每种方法写为一个函数，便于使用的时候直接调用。
点云读取
提取指定范围内的点云数据(ROI点云提取)，再将提取的数据转为pcl格式的点云。
from pclpy import pcl as pl
def LoadPointCloud(path,start, end):
points = pl.PointCloud.PointXYZRGB()
pl.io.load(path,points)
data = np.zeros((points.size(),3),dtype=np.float32)
data[:, 0] = points.x
data[:, 1] = points.y
data[:, 2] = points.z
print("-------ROI data:{},{}".format(start, end))
print("X max:{} X min:{} Y max:{} Y min:{}".format(np.max(data[:, 0]),np.min(data[:, 0]),np.max(data[:, 1]),np.min(data[:, 1])))
data = data[(data[:, 0] > start[0])]
data = data[(data[:, 0] < end[0])]
data = data[(data[:, 1] < end[1])]
data = data[(data[:, 1] > start[1])]
points_region = pl.PointCloud.PointXYZ(data.reshape(-1,3))
点云显示
1、使用pl.visualization.PCLVisualizer进行点云显示
def PointCouldViewXYZ(points):
# 显示点云
viewer = pl.visualization.PCLVisualizer("CloudPoints")
# 根据Z轴显示点云颜色
fildColor = pl.visualization.PointCloudColorHandlerGenericField.PointXYZ(points, "z")
viewer.addPointCloud(points,fildColor,"sample cloud")
viewer.setBackgroundColor(0., 0., 0.)
while (not viewer.wasStopped()):
viewer.spinOnce(10)
点云法线计算
def compute_normal(cloud,ksearch=18,radius=0.7):
# 计算点云法向量
KdTree = pl.search.KdTree.PointXYZ()
normals = pl.PointCloud.Normal()
normal_estimator = pl.features.NormalEstimationOMP.PointXYZ_Normal()
normal_estimator.setSearchMethod(KdTree)
# normal_estimator.setRadiusSearch(radius)
normal_estimator.setInputCloud(cloud)
normal_estimator.setKSearch(ksearch)
normal_estimator.compute(normals)
return normals
根据索引提取点云
def getClusterPointsFromIndices(cloud,indices):
child_cloud = pl.PointCloud.PointXYZ() # 根据索引批量提取点
extract = pl.filters.ExtractIndices.PointXYZ()
extract.setInputCloud(cloud)
pi = pl.vectors.Int(indices.indices)
extract.setNegative(False) # 如果设为true,可以提取指定index之外的点云
extract.setIndices(pi)
extract.filter(child_cloud)
return child_cloud
点云统计滤波
def points_filter(points):
sor = pl.filters.StatisticalOutlierRemoval.PointXYZ()
cloud = pl.PointCloud.PointXYZ()
sor.setInputCloud(points)
sor.setMeanK(10)
sor.setStddevMulThresh(10)
sor.filter(cloud)
点云区域分割
def ChildRegionGrow(cloud,MinS=30,KSearch=30,n_neighbours=30,
smooth_threshold=5,curvature_threshold=0.1,residual_threshold = 0.3):
tree = pl.search.KdTree.PointXYZ()
tree.setInputCloud(cloud)
normals = pl.PointCloud.Normal()
ne = pl.features.NormalEstimationOMP.PointXYZ_Normal()
ne.setInputCloud(cloud)
ne.setSearchMethod(tree)
# ne.setRadiusSearch(0.7)
ne.setKSearch(KSearch)
ne.compute(normals)
rg = pl.segmentation.RegionGrowing.PointXYZ_Normal()
rg.setInputCloud(cloud)
rg.setInputNormals(normals)
rg.setSearchMethod(tree)
rg.setMinClusterSize(MinS)
rg.setMaxClusterSize(cloud.size())
# rg.setNumberOfNeighbours(n_neighbours) # 注释
rg.setResidualThreshold(residual_threshold)
rg.setSmoothnessThreshold(smooth_threshold / 180 * math.pi) # / 180 * math.pi
rg.setCurvatureThreshold(curvature_threshold)
child_cluster_indices = pl.vectors.PointIndices()
rg.extract(child_cluster_indices)
return child_cluster_indices
点云密度聚类
def DensityBasedSpatialClustering(data):
from sklearn.cluster import DBSCAN
dbscan = DBSCAN(eps=1,min_samples=10,metric="chebyshev")
X = data[:,:2]
dbscan.fit(data)
y = dbscan.labels_
print(y)
plt.scatter(X[:, 0], X[:, 1], c=y)
plt.show()
点云超体素聚类
def Supervoxel(path):
cloud_filtered = pl.PointCloud.PointXYZRGBA()
pl.io.load(path,cloud_filtered )
normal = cloud_filtered.compute_normals(radius=0.3)
sup = pl.segmentation.SupervoxelClustering.PointXYZRGBA(voxel_resolution=1,seed_resolution=2)
sup.setNormalCloud(normal)
sup.setInputCloud(cloud_filtered)
sup.setNormalImportance(4.0)
sup.setSpatialImportance(1.0)
sup.setColorImportance(0.0)
super_clusters = pl.vectors.map_uint32t_PointXYZRGBA()
sup.extract(super_clusters)
voxel_centroid_cloud = sup.getVoxelCentroidCloud()
点云曲率计算
def ShowPointNormal(cloud,k = 10):
# 计算法线
tree = pl.search.KdTree.PointXYZ()
normals = pl.PointCloud.Normal()
ne = pl.features.NormalEstimationOMP.PointXYZ_Normal()
ne.setInputCloud(cloud)
ne.setSearchMethod(tree)
# ne.setRadiusSearch(0.7)
ne.setKSearch(k)
ne.compute(normals)
# 计算曲率
cur = pl.features.PrincipalCurvaturesEstimation.PointXYZ_Normal_PrincipalCurvatures()
curvatures = pl.PointCloud.PrincipalCurvatures()
cur.setInputCloud(cloud)
cur.setInputNormals(normals)
cur.setKSearch(k)
cur.setSearchMethod(tree)
cur.compute(curvatures)
return normals,curvatures
点云保存
pl.io.savePLYFile(savePath,points)
点云颜色值修改
随机给点云赋予颜色、也可指定颜色
def rgb2c(r, g, b):
return r << 16 | g << 8 | b
def(indices,child_cloud,points_cluster )
r = int(np.random.randint(50, 255,dtype=np.uint8)* np.random.uniform(0.2,1))
g = int(np.random.randint(50, 255,dtype=np.uint8)* np.random.uniform(0.2,1))
b = int(np.random.randint(50, 255,dtype=np.uint8)* np.random.uniform(0.2,1))
rgb = rgb2c(r, g, b)
points_cluster[:, 0] = child_cloud.x
points_cluster[:, 1] = child_cloud.y
points_cluster[:, 2] = child_cloud.z
points_cluster[:, 3] = rgb
return points_cluster;
深度图转点云
def Depth2Points3d(depth_data):
c_x = 642.771
c_y = 365.479
fx = 926.134
fy = 926.12
res_data = np.zeros((depth_data.shape[0],depth_data.shape[1],3))
for u in range(depth_data.shape[0]):
for v in range(depth_data.shape[1]):
z = depth_data[u, v]
x = (v - c_x) * z / fx
y = (u - c_y) * z / fy
res_data[u, v] = (x, y, z)
return res_data