点云分割
第一章 点云数据采集
1. 点云分割
点云数据中包含目标物体，点云分割算法即将物体分割出来。
2 分割算法
2.1 RANSAC(随机采样一致性)方法
基于随机采样一致性的分割的步骤如下：
1.从一个样本集S中，随机抽取n个样本，拟合出一个模型，n是能够初始化模型的最小样本数。
2.用1中得到的模型去测试所有的其它数据，如果某个点与模型的误差小于某个阈值，则该点适用于这个模型，认为它也是局内点。
3.如果模型内的局内点达到一定个数，那么估计的模型就足够合理。
4.用所有假设的局内点去重新执行1,2，估计模型，因为它仅仅被初始的假设局内点估计过。
5.最后，通过估计局内点与模型的错误率来评估模型。
平面分割
open3d
import open3d as o3d
import numpy as np
# 读取点云数据
pcd = o3d.io.read_point_cloud("second_radius_cloud.pcd")
# 使用RANSAC进行平面分割
plane_model, inliers = pcd.segment_plane(distance_threshold=10,
ransac_n=3,
num_iterations=1000)
# 获取分割出的平面的点云
inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0.5, 0]) # 红色表示平面
# 获取其它点云
outlier_cloud = pcd.select_by_index(inliers, invert=True)
# 可视化
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud],
zoom=0.8,
front=[-0.4999, -0.1659, -0.8499],
lookat=[2.1813, 2.0619, 2.0999],
up=[0.1204, -0.9852, 0.1215])
PCL
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
int main ()
{
// 1. 读取点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("second_radius_cloud.pcd", *cloud) == -1)
{
PCL_ERROR ("Couldn't read the file second_radius_cloud.pcd \n");
return (-1);
}
// 2. 创建分割对象和设置参数
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (10);
// 3. 进行分割
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
return (-1);
}
// 4. 提取平面
pcl::ExtractIndices<pcl::PointXYZ> extract;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_plane);
// 5. 可视化
pcl::visualization::PCLVisualizer viewer("PCL Plane Segmenter");
viewer.setBackgroundColor(0, 0, 0);
// 可视化原始点云
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler(cloud, 255, 255, 255);
viewer.addPointCloud(cloud, cloud_color_handler, "original_cloud");
// 可视化平面
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> plane_color_handler(cloud_plane, 0, 255, 0);
viewer.addPointCloud(cloud_plane, plane_color_handler, "plane_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane_cloud");
viewer.initCameraParameters();
viewer.saveScreenshot("screenshot.png");
viewer.spin ();
return 0;
}
2.2 区域生长分割
随机种下曲率较小的种子后进行延申，根据种子邻域与法线之间的角度与阈值比较，从而判断是否处于哪个领域。
open3d
类RegionGrowing.py
import open3d as o3d
import numpy as np
from collections import deque
class RegionGrowing:
# 构造函数
def __init__(self, cloud,
min_pts_per_cluster=1, # 每个聚类的最小点数
max_pts_per_cluster=np.inf, # 每个聚类的最大点数
theta_threshold=30, # 法向量夹角阈值
curvature_threshold=0.05, # 曲率阈值
neighbour_number=30, # 邻域搜索点数
point_neighbours=[], # 近邻点集合
point_labels=[], # 点标签
num_pts_in_segment=[], # 分类标签
clusters=[], # 聚类容器
number_of_segments=0): # 聚类个数
self.cure = None # 存储每个点曲率的容器
self.pcd = cloud # 输入点云
self.min_pts_per_cluster = min_pts_per_cluster
self.max_pts_per_cluster = max_pts_per_cluster
self.theta_threshold = np.deg2rad(theta_threshold)
self.curvature_threshold = curvature_threshold
self.neighbour_number = neighbour_number
self.point_neighbours = point_neighbours
self.point_labels = point_labels
self.num_pts_in_segment = num_pts_in_segment
self.clusters = clusters
self.number_of_segments = number_of_segments
# -------------------------------------参数准备--------------------------------------
def prepare_for_segment(self):
points = np.asarray(self.pcd.points) # 点坐标
normals = np.asarray(self.pcd.normals) # 法向量
# 判断点云是否为空
if not points.shape[0]:
return False
# 判断是否有近邻点
if self.neighbour_number == 0:
return False
# 点云需要包含法向量信息
if points.shape[0] != normals.shape[0]:
self.pcd.estimate_normals(o3d.geometry.KDTreeSearchParamKNN(self.neighbour_number))
return True
# ------------------------------------近邻点搜索-------------------------------------
def find_neighbour_points(self):
number = len(self.pcd.points)
kdtree = o3d.geometry.KDTreeFlann(self.pcd)
self.point_neighbours = np.zeros((number, self.neighbour_number))
for ik in range(number):
[_, idx, _] = kdtree.search_knn_vector_3d(self.pcd.points[ik], self.neighbour_number) # K近邻搜索
self.point_neighbours[ik, :] = idx
# -----------------------------------判意点所属分类-----------------------------------
def validate_points(self, point, nebor):
is_seed = True
cosine_threshold = np.cos(self.theta_threshold) # 法向量夹角（平滑）阈值
curr_seed_normal = self.pcd.normals[point] # 当前种子点的法向量
seed_nebor_normal = self.pcd.normals[nebor] # 种子点邻域点的法向量
dot_normal = np.fabs(np.dot(seed_nebor_normal, curr_seed_normal))
# 如果小于平滑阈值
if dot_normal < cosine_threshold:
return False, is_seed
# 如果小于曲率阈值
if self.cure[nebor] > self.curvature_threshold:
is_seed = False
return True, is_seed
# ----------------------------------对点附上分类标签----------------------------------
def label_for_points(self, initial_seed, segment_number):
seeds = deque([initial_seed])
self.point_labels[initial_seed] = segment_number
num_pts_in_segment = 1
while len(seeds):
curr_seed = seeds[0]
seeds.popleft()
i_nebor = 0
while i_nebor < self.neighbour_number and i_nebor < len(self.point_neighbours[curr_seed]):
index = int(self.point_neighbours[curr_seed, i_nebor])
if self.point_labels[index] != -1:
i_nebor += 1
continue
belongs_to_segment, is_seed = self.validate_points(curr_seed, index)
if not belongs_to_segment:
i_nebor += 1
continue
self.point_labels[index] = segment_number
num_pts_in_segment += 1
if is_seed:
seeds.append(index)
i_nebor += 1
return num_pts_in_segment
# ------------------------------------区域生长过程------------------------------------
def region_growing_process(self):
num_of_pts = len(self.pcd.points) # 点云点的个数
self.point_labels = -np.ones(num_of_pts) # 初始化点标签
self.pcd.estimate_covariances(o3d.geometry.KDTreeSearchParamKNN(self.neighbour_number))
cov_mat = self.pcd.covariances # 获取每个点的协方差矩阵
self.cure = np.zeros(num_of_pts) # 初始化存储每个点曲率的容器
# 计算每个点的曲率
for i_n in range(num_of_pts):
eignvalue, _ = np.linalg.eig(cov_mat[i_n]) # SVD分解求特征值
idx = eignvalue.argsort()[::-1]
eignvalue = eignvalue[idx]
self.cure[i_n] = eignvalue[2] / (eignvalue[0] + eignvalue[1] + eignvalue[2])
point_curvature_index = np.zeros((num_of_pts, 2))
for i_cu in range(num_of_pts):
point_curvature_index[i_cu, 0] = self.cure[i_cu]
point_curvature_index[i_cu, 1] = i_cu
# 按照曲率大小进行排序
temp_cure = np.argsort(point_curvature_index[:, 0])
point_curvature_index = point_curvature_index[temp_cure, :]
seed_counter = 0
seed = int(point_curvature_index[seed_counter, 1]) # 选取曲率最小值点
segmented_pts_num = 0
number_of_segments = 0
while segmented_pts_num < num_of_pts:
pts_in_segment = self.label_for_points(seed, number_of_segments) # 根据种子点进行分类
segmented_pts_num += pts_in_segment
self.num_pts_in_segment.append(pts_in_segment)
number_of_segments += 1
# 寻找下一个种子
for i_seed in range(seed_counter + 1, num_of_pts):
index = int(point_curvature_index[i_seed, 1])
if self.point_labels[index] == -1:
seed = index
seed_counter = i_seed
break
# ----------------------------------根据标签进行分类-----------------------------------
def region_growing_clusters(self):
number_of_segments = len(self.num_pts_in_segment)
number_of_points = np.asarray(self.pcd.points).shape[0]
# 初始化聚类数组
for i in range(number_of_segments):
tmp_init = list(np.zeros(self.num_pts_in_segment[i]))
self.clusters.append(tmp_init)
counter = list(np.zeros(number_of_segments))
for i_point in range(number_of_points):
segment_index = int(self.point_labels[i_point])
if segment_index != -1:
point_index = int(counter[segment_index])
self.clusters[segment_index][point_index] = i_point
counter[segment_index] = point_index + 1
self.number_of_segments = number_of_segments
# ----------------------------------执行区域生长算法-----------------------------------
def extract(self):
if not self.prepare_for_segment():
print("区域生长算法预处理失败！")
return
self.find_neighbour_points()
self.region_growing_process()
self.region_growing_clusters()
# 根据设置的最大最小点数筛选符合阈值的分类
all_cluster = []
for i in range(len(self.clusters)):
if self.min_pts_per_cluster <= len(self.clusters[i]) <= self.max_pts_per_cluster:
all_cluster.append(self.clusters[i])
else:
self.point_labels[self.clusters[i]] = -1
self.clusters = all_cluster
return all_cluster
主程序
import open3d as o3d
import numpy as np
import regiongrowing as reg
# ------------------------------读取点云---------------------------------------
pcd = o3d.io.read_point_cloud("second_radius_cloud.pcd")
# ------------------------------区域生长---------------------------------------
rg = reg.RegionGrowing(pcd,
min_pts_per_cluster=500, # 每个聚类的最小点数
max_pts_per_cluster=100000, # 每个聚类的最大点数
neighbour_number=30, # 邻域搜索点数
theta_threshold=30, # 平滑阈值（角度制）
curvature_threshold=0.05) # 曲率阈值
# ---------------------------聚类结果分类保存----------------------------------
indices = rg.extract()
print("聚类个数为", len(indices))
segment = [] # 存储分割结果的容器
for i in range(len(indices)):
ind = indices[i]
clusters_cloud = pcd.select_by_index(ind)
r_color = np.random.uniform(0, 1, (1, 3)) # 分类点云随机赋色
clusters_cloud.paint_uniform_color([r_color[:, 0], r_color[:, 1], r_color[:, 2]])
segment.append(clusters_cloud)
# -----------------------------结果可视化------------------------------------
o3d.visualization.draw_geometries(segment, window_name="区域生长分割",
width=1024, height=768,
left=50, top=50,
mesh_show_back_face=True)
PCL
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/region_growing.h>
int main ()
{
// 1. Load point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read the file second_radius_cloud.pcd \n");
return (-1);
}
// 2. Estimate normals
pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(cloud);
normal_estimator.setKSearch(300);
normal_estimator.compute(*normals);
// 3. Apply region growing segmentation
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setMinClusterSize(500);
reg.setMaxClusterSize(1000000);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(30);
reg.setInputCloud(cloud);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(30); // 3 degrees
reg.setCurvatureThreshold(0.05);
std::vector<pcl::PointIndices> clusters;
reg.extract(clusters);
// 4. Visualize the result
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Region Growing Segmentation"));
viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud, "region growing");
viewer->setBackgroundColor(0, 0, 0);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "region growing");
viewer->initCameraParameters();
viewer->saveScreenshot("screenshot.png");
viewer->spin();
return 0;
}
2.3 欧几里得聚类分割
根据欧几里得距离的大小将点进行聚类。
open3d
import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt
def main():
pcd = o3d.io.read_point_cloud("second_radius_cloud.pcd")
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=100, max_nn=30))
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
labels = np.array(pcd.cluster_dbscan(eps=20, 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])
if __name__ == "__main__":
main()
PCL
#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
int main ()
{
// 1. Load the point cloud data from file
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read the file second_radius_cloud.pcd \n");
return (-1);
}
// 2. Create the KD-Tree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud);
// 3. Perform the Euclidean Cluster Extraction
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(2);
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);
// 4. Visualize the result
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Euclidean Clustering"));
viewer->setBackgroundColor(0, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>());
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
{
pcl::PointXYZRGB point;
point.x = cloud->points[*pit].x;
point.y = cloud->points[*pit].y;
point.z = cloud->points[*pit].z;
point.r = j * 23 % 255;
point.g = j * 77 % 255;
point.b = j * 129 % 255;
colored_cloud->points.push_back(point);
}
j++;
}
viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud, "Euclidean Clustering");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "Euclidean Clustering");
viewer->spin();
return 0;
}
PCL
#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
int main ()
{
// 1. Load the point cloud data from file
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read the file second_radius_cloud.pcd \n");
return (-1);
}
// 2. Create the KD-Tree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud);
// 3. Perform the Euclidean Cluster Extraction
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(2);
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);
// 4. Visualize the result
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Euclidean Clustering"));
viewer->setBackgroundColor(0, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>());
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
{
pcl::PointXYZRGB point;
point.x = cloud->points[*pit].x;
point.y = cloud->points[*pit].y;
point.z = cloud->points[*pit].z;
point.r = j * 23 % 255;
point.g = j * 77 % 255;
point.b = j * 129 % 255;
colored_cloud->points.push_back(point);
}
j++;
}
viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud, "Euclidean Clustering");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "Euclidean Clustering");
viewer->spin();
return 0;
}
2.4 霍夫变换分割
将霍夫变换从2D延申到了3D。
PCL
圆柱体检测
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/sac_segmentation.h>
int main()
{
// 1. Load Point Cloud data
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read file second_radius_cloud.pcd \n");
return (-1);
}
// 2. Estimate Point Normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod(tree);
ne.setInputCloud(cloud);
ne.setKSearch(50);
ne.compute(*normals);
// 3. Apply Hough Transform based cylinder segmentation
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_CYLINDER);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setNormalDistanceWeight(0.1);
seg.setMaxIterations(10000);
seg.setDistanceThreshold(50);
seg.setRadiusLimits(0.0, 0.1);
seg.setInputCloud(cloud);
seg.setInputNormals(normals);
seg.segment(*inliers_cylinder, *coefficients_cylinder);
// 4. Visualize the Cylinder
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Hough Transform Cylinder Segmentation"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cylinder(new pcl::PointCloud<pcl::PointXYZ>);
viewer->addPointCloud<pcl::PointXYZ>(cloud_cylinder, "cylinder", 0);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0, 0, "cylinder");
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
}
return 0;
}
