Udacity机器人软件工程师课程笔记（二十一) - 对点云进行集群可视化 - 聚类的分割 - K-means|K均值聚类, DBSCAN算法

聚类的分割

1.K-均值聚类

（1）K-均值聚类介绍

k均值聚类算法（k-means clustering algorithm）是一种迭代求解的聚类分析算法，其步骤是随机选取K个对象作为初始的聚类中心，然后计算每个对象与各个种子聚类中心之间的距离，把每个对象分配给距离它最近的聚类中心。

聚类中心以及分配给它们的对象就代表一个聚类。每分配一个样本，聚类的聚类中心会根据聚类中现有的对象被重新计算。这个过程将不断重复直到满足某个终止条件。终止条件可以是没有（或最小数目）对象被重新分配给不同的聚类，没有（或最小数目）聚类中心再发生变化，误差平方和局部最小。

在具有不同k值的同一数据集上运行k均值聚类的示例（注意：在k = 50的图中，颜色会重复出现，但表示单独的聚类）。

（2）k - means算法

先随机选取K个对象作为初始的聚类中心。然后计算每个对象与各个种子聚类中心之间的距离，把每个对象分配给距离它最近的聚类中心。聚类中心以及分配给它们的对象就代表一个聚类。一旦全部对象都被分配了，每个聚类的聚类中心会根据聚类中现有的对象被重新计算。这个过程将不断重复直到满足某个终止条件。终止条件可以是以下任何一个：
1)没有（或最小数目）对象被重新分配给不同的聚类。
2)没有（或最小数目）聚类中心再发生变化。
3)误差平方和局部最小。

---

伪代码：
假设我们有$n$个数据点，要将其分成$k$类。

1. 先随机选取$k$个点$c_1,c_2,...,c_k$作为初始的聚类中心。
2. 定义收敛/终止标准(解决方案的稳定性和最大迭代次数)
3. while 不符合收敛/终止准则 do:
   1. for $i=1$ to $n$:
      计算$p_i$到每个簇中心的距离
      分配$p_i$到最接近它的质心，并相应地标记它
      endfor
   2. for $j=1$ to $k$:
      根据所有数据点的平均值重新计算$j$的质心
      endfor
      endwhile

（3）k -means 代码

首先给出完整的代码：

import numpy as np
import matplotlib.pyplot as plt
import cv2


# 定义一个函数来生成集群
def cluster_gen(n_clusters, pts_minmax=(10, 100), x_mult=(1, 4), y_mult=(1, 3),
                x_off=(0, 50), y_off=(0, 50)):
    # n_clusters = 要生成的集群数量
    # pts_minmax = 每个集群的点数范围
    # x_mult = 乘法器的范围，在x方向修改集群的大小
    # y_mult = 乘法器的范围，在y方向修改集群的大小
    # x_off = 簇在x方向上的位置偏移范围
    # y_off = 簇在y方向上的位置偏移范围

    # 初始化一些空列表以接收集群成员位置
    clusters_x = []
    clusters_y = []

    # 在给定的参数范围内生成随机值
    n_points = np.random.randint(pts_minmax[0], pts_minmax[1], n_clusters)
    x_multipliers = np.random.randint(x_mult[0], x_mult[1], n_clusters)
    y_multipliers = np.random.randint(y_mult[0], y_mult[1], n_clusters)
    x_offsets = np.random.randint(x_off[0], x_off[1], n_clusters)
    y_offsets = np.random.randint(y_off[0], y_off[1], n_clusters)

    # 生成随机集群给定参数值
    for idx, npts in enumerate(n_points):
        xpts = np.random.randn(npts) * x_multipliers[idx] + x_offsets[idx]
        ypts = np.random.randn(npts) * y_multipliers[idx] + y_offsets[idx]
        clusters_x.append(xpts)
        clusters_y.append(ypts)

    # 返回集群的位置
    return clusters_x, clusters_y


# 生成一些集群
n_clusters = 7
clusters_x, clusters_y = cluster_gen(n_clusters)
# 转换为OpenCV格式的单个数据集
data = np.float32((np.concatenate(clusters_x), np.concatenate(clusters_y))).transpose()

# k - means参数定义
# 要定义的集群数量
k_clusters = 7
# 要执行的最大迭代数
max_iter = 10
# 停止迭代的精度准则
epsilon = 1.0
# 以OpenCV格式定义标准
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
# 调用数据集上的k-means算法
compactness, label, center = cv2.kmeans(data, k_clusters, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS)

# 定义一些空列表来接收k-means集群点
kmeans_clusters_x = []
kmeans_clusters_y = []

# 从输出中提取k-means集群
for idx in range(k_clusters):
    kmeans_clusters_x.append(data[label.ravel() == idx][:, 0])
    kmeans_clusters_y.append(data[label.ravel() == idx][:, 1])

# 绘制原始集群与k-means集群的比较
fig = plt.figure(figsize=(12, 6))
plt.subplot(121)
min_x = np.min(data[:, 0])
max_x = np.max(data[:, 0])
min_y = np.min(data[:, 1])
max_y = np.max(data[:, 1])
for idx, xpts in enumerate(clusters_x):
    plt.plot(xpts, clusters_y[idx], 'o')
    plt.xlim(min_x, max_x)
    plt.ylim(min_y, max_y)
    plt.title('Original Clusters', fontsize=20)
plt.subplot(122)
for idx, xpts in enumerate(kmeans_clusters_x):
    plt.plot(xpts, kmeans_clusters_y[idx], 'o')
    plt.xlim(min_x, max_x)
    plt.ylim(min_y, max_y)
    plt.title('k-means Clusters', fontsize=20)
fig.tight_layout()
plt.subplots_adjust(left=0.03, right=0.98, top=0.9, bottom=0.05)
plt.show()

---

代码解释

K-means集群是一种强大的工具，但也有其局限性。Udacity提供了了一个沙箱环境，我们可以在其中测试k-means算法的局限。

# 定义一个函数来生成集群
def cluster_gen(n_clusters, pts_minmax=(10, 100), x_mult=(1, 4), y_mult=(1, 3),
                x_off=(0, 50), y_off=(0, 50)):
    # n_clusters = 要生成的集群数量
    # pts_minmax = 每个集群的点数范围
    # x_mult = 乘法器的范围，在x方向修改集群的大小
    # y_mult = 乘法器的范围，在y方向修改集群的大小
    # x_off = 簇在x方向上的位置偏移范围
    # y_off = 簇在y方向上的位置偏移范围

    # 初始化一些空列表以接收集群成员位置
    clusters_x = []
    clusters_y = []

    # 在给定的参数范围内生成随机值
    n_points = np.random.randint(pts_minmax[0], pts_minmax[1], n_clusters)
    x_multipliers = np.random.randint(x_mult[0], x_mult[1], n_clusters)
    y_multipliers = np.random.randint(y_mult[0], y_mult[1], n_clusters)
    x_offsets = np.random.randint(x_off[0], x_off[1], n_clusters)
    y_offsets = np.random.randint(y_off[0], y_off[1], n_clusters)

    # 生成随机集群给定参数值
    for idx, npts in enumerate(n_points):
        xpts = np.random.randn(npts) * x_multipliers[idx] + x_offsets[idx]
        ypts = np.random.randn(npts) * y_multipliers[idx] + y_offsets[idx]
        clusters_x.append(xpts)
        clusters_y.append(ypts)

    # 返回集群的位置
    return clusters_x, clusters_y

首先要做的是生成一些集群。现在提供了一个名为cluster_gen()的函数，它将从随机高斯分布生成简单的集群总体。

numpy.random.randint(low, high=None, size=None, dtype='l')

函数的作用是，返回一个随机整型数，范围从低（包括）到高（不包括），即[low, high)。

如果没有写参数high的值，则返回[0,low)的值。

np.random.randn(d0,d1,d2……dn)

1. 当函数括号内没有参数时，则返回一个浮点数；
2. 当函数括号内有一个参数时，则返回秩为1的数组，不能表示向量和矩阵；
3. 当函数括号内有两个及以上参数时，则返回对应维度的数组，能表示向量或矩阵；

---

生成集群后，需要将数据转换为OpenCV所需要的格式，然后定义k-means算法的约束，最后调用该cv2.kmeans()函数。

# 生成一些集群
n_clusters = 7
clusters_x, clusters_y = cluster_gen(n_clusters)
# 转换为OpenCV格式的单个数据集
data = np.float32((np.concatenate(clusters_x), np.concatenate(clusters_y))).transpose()

# k - means参数定义
# 要定义的集群数量
k_clusters = 7
# 要执行的最大迭代数
max_iter = 10
# 停止迭代的精度准则
epsilon = 1.0
# 以OpenCV格式定义标准
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, max_iter, epsilon)
# 调用数据集上的k-means算法
compactness, label, center = cv2.kmeans(data, k_clusters, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS)

关于opencv下的kmean算法，函数为cv2.kmeans():

函数的格式为：kmeans(data, K, bestLabels, criteria, attempts, flags)

1. data: 分类数据。
2. K: 分类数，opencv2的kmeans分类是需要已知分类数的。
3. bestLabels：预设的分类标签：没有的话None
4. criteria：迭代停止的模式选择，这是一个含有三个元素的元组型数。格式为（type,max_iter,epsilon）
   其中，type又有两种选择：
   cv2.TERM_CRITERIA_EPS:精确度（误差）满足epsilon停止。
   cv2.TERM_CRITERIA_MAX_ITER：迭代次数超过max_iter停止。
   cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER，两者合体，任意一个满足结束。
5. attempts：重复试验kmeans算法次数，将会返回最好的一次结果
6. flags：初始类中心选择，两种方法：cv2.KMEANS_PP_CENTERS; cv2.KMEANS_RANDOM_CENTERS

---

程序输出如下:

2. DBSCAN算法

（1）DBSCAN算法介绍

左侧是原始数据，右侧是由DBSCAN算法识别的聚类。对于DBSCAN集群，大色点表示核心集群成员，小色点表示集群边缘成员，小黑点表示离群值。

DBSCAN代表带噪声的应用程序的基于密度的空间聚类。当我们不知道数据中预期有多少个聚类时，该算法是k均值的一种很好的替代方法，但是我们需要知道有关点应如何按照密度（聚类中点之间的距离）进行聚类的知识。

DBSCAN算法通过对在某个阈值距离内的数据点进行分组来创建聚类 $d_{th}$ 从数据中最近的其他点开始。
该算法有时也称为“欧几里得聚类”，因为是否在特定聚类中放置点的决定是基于该点与其他聚类成员之间的“欧几里得距离”。

欧几里得距离是连接两个点的直线的长度，但是定义数据中点位置的坐标不必是空间坐标。例如，可以在颜色空间或数据的任何其他特征空间中定义它们。

点之间的欧几里得距离$\mathbf{p}$和$\mathbf{q}$在n维数据集中，其中的位置$\mathbf{p}$由坐标定义$（p_1，p_2，\dots ，p_n）$和位置$\mathbf{q}$定义为$（q_1，q_2，\dots ，q_n）$，那么两点之间的距离就是：

$D=\sqrt{(p_1-q_1{)}^2+(p_2-q_2{)}^2+...+(p_n-q_n{)}^2}$

（2）DBSCAN过滤算法

假设有一n个点$p_1,p_2,...,p_n$的数据集$P$:

1. 为组成集群的最小点数设置约束(min_samples)
2. 设置距离阈值或聚类点之间的最大距离(max_dist)
3. For every point $p_i$ in $P$, do:
   • if $p_i$ has at least one neighbor within max_dist :
     • if $p_i$'s neighbot ist part of a cluster:
       • add $p_i$ to that cluster
     • if $p_i$ has at least min_samples-1 netghbors within max_dist:
       • $p_i$ becomes a “core member” of the cluster
     • else:
       • $p_i$ becomes an “edge member” of the cluster
   • else:
     • $p_i$ is defined as an outlier

上面的代码做了一些简化，以帮助理解算法。如果随机访问每个数据点，最终将创建比所需数量更多的群集，并且必须以某种方式合并它们。我们可以通过控制访问数据点的顺序来避免这种情况。

使用我们的第一个合格数据点创建新集群后，我们会将其所有邻居添加到集群中。然后，我们将添加它的邻居的邻居以及他们的邻居，直到我们访问了属于该集群的每个数据点。只有这样做之后，我们才能继续随机选择另一个数据点。这样，我们可以保证集群是完整的，并且数据集中不再有属于该集群的点。

它的工作原理如下：首先我们选择两个参数，正数epsilon和自然数minPoints。然后，我们从数据集中选择任意点开始。如果距epsilon的距离内的minPoints个点以上（包括原始点本身），则我们将所有这些点都视为“群集”的一部分。然后，我们通过检查所有新点并查看它们在epsilon距离内是否也拥有超过minPoints点来扩展该群集，如果是，则以递归方式扩展该群集。

最终，我们用完了要添加到群集中的点。然后，我们选择一个新的任意点并重复该过程。现在，完全有可能我们选择的一个点在其epsilon球中少于minPoints，并且也不属于任何其他群集。如果是这种情况，则将其视为不属于任何群集的“噪声点”。

DBSCAN算法的完整伪代码实现可以在DBSCAN Wikipedia页面上找到。

对于其中min_samples= 4 的示例，以下是结果的外观外观：

蓝点是在max_dist内至少有三个邻居的核心集群成员（满足邻居数+自身>= min_samples)。黄点是有邻居的边缘成员，但少于三个。黑点是没有邻居的异常值。

（3）相关代码

import numpy as np
import matplotlib.pyplot as plt
import cv2
from sklearn.cluster import DBSCAN

# 定义一个函数来生成集群
def cluster_gen(n_clusters, pts_minmax=(10, 100), x_mult=(1, 4), y_mult=(1, 3),
                x_off=(0, 50), y_off=(0, 50)):
    # n_clusters = 要生成的集群数量
    # pts_minmax = 每个集群的点数范围
    # x_mult = 乘法器的范围，在x方向修改集群的大小
    # y_mult = 乘法器的范围，在y方向修改集群的大小
    # x_off = 簇在x方向上的位置偏移范围
    # y_off = 簇在y方向上的位置偏移范围

    # 初始化一些空列表以接收集群成员位置
    clusters_x = []
    clusters_y = []

    # 在给定的参数范围内生成随机值
    n_points = np.random.randint(pts_minmax[0], pts_minmax[1], n_clusters)
    x_multipliers = np.random.randint(x_mult[0], x_mult[1], n_clusters)
    y_multipliers = np.random.randint(y_mult[0], y_mult[1], n_clusters)
    x_offsets = np.random.randint(x_off[0], x_off[1], n_clusters)
    y_offsets = np.random.randint(y_off[0], y_off[1], n_clusters)

    # 生成随机集群给定参数值
    for idx, npts in enumerate(n_points):
        xpts = np.random.randn(npts) * x_multipliers[idx] + x_offsets[idx]
        ypts = np.random.randn(npts) * y_multipliers[idx] + y_offsets[idx]
        clusters_x.append(xpts)
        clusters_y.append(ypts)

    # 返回集群的位置
    return clusters_x, clusters_y

# 生成一些集群
n_clusters = 20
clusters_x, clusters_y = cluster_gen(n_clusters)
# 转换为OpenCV格式的单个数据集
data = np.float32((np.concatenate(clusters_x), np.concatenate(clusters_y))).transpose()
# 定义 max_distance (eps parameter in DBSCAN())
max_distance = 2
db = DBSCAN(eps=max_distance, min_samples=10).fit(data)
# 提取核心集群成员的掩码
core_samples_mask = np.zeros_like(db.labels_, dtype=bool)
core_samples_mask[db.core_sample_indices_] = True
# 提取标签 (-1 is used for outliers)
labels = db.labels_
n_clusters = len(set(labels)) - (1 if -1 in labels else 0)
unique_labels = set(labels)

# 计算结果
min_x = np.min(data[:, 0])
max_x = np.max(data[:, 0])
min_y = np.min(data[:, 1])
max_y = np.max(data[:, 1])

fig = plt.figure(figsize=(12,6))
plt.subplot(121)
plt.plot(data[:,0], data[:,1], 'ko')
plt.xlim(min_x, max_x)
plt.ylim(min_y, max_y)
plt.title('Original Data', fontsize = 20)

plt.subplot(122)
# 下面是绘制核心、边缘和异常值的一种方法
# Credit to: http://scikit-learn.org/stable/auto_examples/cluster/plot_dbscan.html#sphx-glr-auto-examples-cluster-plot-dbscan-py
colors = [plt.cm.Spectral(each) for each in np.linspace(0, 1, len(unique_labels))]
for k, col in zip(unique_labels, colors):
    if k == -1:
        # 黑色用于噪点.
        col = [0, 0, 0, 1]

    class_member_mask = (labels == k)

    xy = data[class_member_mask & core_samples_mask]
    plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col),
             markeredgecolor='k', markersize=7)

    xy = data[class_member_mask & ~core_samples_mask]
    plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col),
             markeredgecolor='k', markersize=3)
plt.xlim(min_x, max_x)
plt.ylim(min_y, max_y)
plt.title('DBSCAN: %d clusters found' % n_clusters, fontsize = 20)
fig.tight_layout()
plt.subplots_adjust(left=0.03, right=0.98, top=0.9, bottom=0.05)
plt.show()

相关结果输出如下：

3.PCL聚类

（1）环境搭建

现在是时候回到ROS，Gazebo和Rviz对点云进行一些聚类分割了。在本练习中，我们将使用PCL库函数EuclideanClusterExtraction()，该函数在3D点云上执行DBSCAN集群搜索。

在本练习中，我们将使用上一课中的相同桌面场景，我们已经在虚拟机（或本地）上设置了Catkin工作区，则第一步是将/sensor_stick目录及其所有内容复制或移动到~/catkin_ws/src。

$ cp -r ~/RoboND-Perception-Exercises/Exercise-2/sensor_stick ~/catkin_ws/src/

接下来，使用rosdep获取运行本练习所需的所有依赖项。

$ cd ~/catkin_ws
$ rosdep install --from-paths src --ignore-src --rosdistro=kicanetic -y

之后，运行catkin_make以建立依赖关系。

$ catkin_make

将以下行添加到.bashrc文件中

export GAZEBO_MODEL_PATH=~/catkin_ws/src/sensor_stick/models
source ~/catkin_ws/devel/setup.bash

运行以下命令以在Gazebo和RViz中启动场景：

$ roslaunch sensor_stick robot_spawn.launch

输出如下：

但是可以看出，有些模型没有被成功加载。

我删掉src目录下的sensor_stick后使用catkin_make重新建立了一遍依赖关系。然后重新执行了操作。在新的Terminator下执行了3次roslaunch之后得到了正常显示，但是依旧报了很多错。猜测有可能是src路径下的依赖冲突了。

以下为正常显示：

我们在这个练习中的目标是编写一个ROS节点，将相机数据作为点云，过滤该点云，然后使用欧几里得聚类对各个对象进行分类。

我们将通过添加代码来说编写节点，将点云数据作为信息发布到名为/sensor_stick/point_cloud的主题上。

以下是我们使用的指导脚本文件：

#!/usr/bin/env python

# Import modules
from pcl_helper import *

# TODO: Define functions as required

# Callback function for your Point Cloud Subscriber
def pcl_callback(pcl_msg):

    # TODO: Initialization

    # TODO: Convert ROS msg to PCL data

    # TODO: Voxel Grid Downsampling

    # TODO: PassThrough Filter


    # TODO: RANSAC Plane Segmentation

    # TODO: Extract inliers and outliers

    # TODO: Euclidean Clustering

    # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately

    # TODO: Convert PCL data to ROS messages

    # TODO: Publish ROS messages


if __name__ == '__main__':

    # TODO: ROS node initialization

    # TODO: Create Subscribers

    # TODO: Create Publishers

    # Initialize color_list
    get_color_list.color_list = []

    # TODO: Spin while node is not shutdown

（2）发布点云

接下来进行一个对发布点云的ros节点进行构建，程序如下

#!/usr/bin/env python

# Import modules
from pcl_helper import *
import rospy
# TODO: Define functions as required

# Callback function for your Point Cloud Subscriber
def pcl_callback(pcl_msg):

    # TODO: Initialization

    # TODO: Convert ROS msg to PCL data

    # TODO: Voxel Grid Downsampling

    # TODO: PassThrough Filter


    # TODO: RANSAC Plane Segmentation

    # TODO: Extract inliers and outliers

    # TODO: Euclidean Clustering

    # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately

    # TODO: Convert PCL data to ROS messages

    # TODO: Publish ROS messages
    pcl_objects_pub.publish(pcl_msg)
    pcl_table_pub.publish(pcl_msg)

if __name__ == '__main__':

    # TODO: ROS node initialization
    rospy.init_node('clustering', anonymous=True)
    # TODO: Create Subscribers
    pcl_sub = rospy.Subscriber('/sensor_stick/point_cloud', pc2.PointCloud2, pcl_callback, queue_size=1)
    # TODO: Create Publishers
    pcl_object_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)
    pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)
    # Initialize color_list
    get_color_list.color_list = []

    # TODO: Spin while node is not shutdown
    while not rospy.is_shutdown():
        rospy.spin()

相关代码解释

初始化ROS节点。始化一个称为“clustering”的新节点。

# TODO: ROS node initialization
rospy.init_node('clustering', anonymous=True)

创建两个新的发布者，分别将表和表中的对象的点云数据发布到名为pcl_table和的主题pcl_objects。

# TODO: Create Publishers
pcl_objects_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)
pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)

创建订阅者。在这里，我们向节点订阅“ sensor_stick / point_cloud”主题。每当消息到达时，消息数据（点云）将被传递给pcl_callback()函数进行处理。

# TODO: Create Subscribers
pcl_sub = rospy.Subscriber("/sensor_stick/point_cloud", pc2.PointCloud2, pcl_callback, queue_size=1)

可以防止节点退出直到主动关闭。

# TODO: Spin while node is not shutdown
while not rospy.is_shutdown():
    rospy.spin()

发布ROS消息pcl_callback()。现在只发布原始的点云本身，但稍后将其更改为与表和对象关联的点云。

# TODO: Publish ROS msg
pcl_objects_pub.publish(pcl_msg)
pcl_table_pub.publish(pcl_msg)

（3）过滤和RANSAC

接下来应用过滤并执行RANSAC平面分割。以下是需要采取的步骤：

1. 通过应用体素网格过滤器对点云进行下采样。
2. 应用通过过滤器以隔离桌子和对象。
3. 执行RANSAC平面拟合以识别桌子。
4. 使用ExtractIndices过滤器来创建包含桌子和对象分开新的点云（将它们命名为cloud_table和cloud_objects）。

完成这些操作后，将这些新的云转换回ROS消息格式，并将它们发布到先前创建的主题中，/pcl_objects和/pcl_table。
程序如下：

#!/usr/bin/env python

# Import modules
from pcl_helper import *
import rospy
import pcl
from sklearn.cluster import DBSCAN
# TODO: Define functions as required

# Callback function for your Point Cloud Subscriber
def pcl_callback(pcl_msg):
    # TODO: Initialization
    # rospy.init_node('pcl_msg', anonymous=True)

    # TODO: Convert ROS msg to PCL data
    cloud = ros_to_pcl(pcl_msg)
    # TODO: Voxel Grid Downsampling
    vox = cloud.make_voxel_grid_filter()
    LEAF_SIZE = 0.01
    vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
    cloud = vox.filter()
    # TODO: PassThrough Filter
    passthrough = cloud.make_passthrough_filter()
    filter_axis = 'z'
    passthrough.set_filter_field_name(filter_axis)
    axis_min = 0.76
    axis_max = 1.3
    passthrough.set_filter_limits(axis_min, axis_max)
    cloud = passthrough.filter()
    # TODO: RANSAC Plane Segmentation
    seg = cloud.make_segmenter()
    seg.set_model_type(pcl.SACMODEL_PLANE)
    seg.set_method_type(pcl.SAC_RANSAC)
    max_distance = 0.01
    seg.set_distance_threshold(max_distance)
    inlier, coefficients = seg.segment()
    # TODO: Extract inliers and outliers
    cloud_table = cloud.extract(inlier, negative=False)
    cloud_objects = cloud.extract(inlier, negative=True)
    # TODO: Euclidean Clustering
    #max_distance = 2
    #db = DBSCAN(eps=max_distance, min_samples=10).fit(data)
    # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately

    # TODO: Convert PCL data to ROS messages
    cloud_table_ros = pcl_to_ros(cloud_table)
    cloud_objects_ros = pcl_to_ros(cloud_objects)
    # TODO: Publish ROS messages
    pcl_objects_pub.publish(cloud_objects_ros)
    pcl_table_pub.publish(cloud_table_ros)


if __name__ == '__main__':
    # TODO: ROS node initialization
    rospy.init_node('clustering', anonymous=True)
    # TODO: Create Subscribers
    pcl_sub = rospy.Subscriber('/sensor_stick/point_cloud', pc2.PointCloud2, pcl_callback, queue_size=1)
    # TODO: Create Publishers
    pcl_objects_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)
    pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)
    # Initialize color_list
    get_color_list.color_list = []

    # TODO: Spin while node is not shutdown
    while not rospy.is_shutdown():
        rospy.spin()

在这个完成过程中发生了一个比较严重的bug，就是我的命名空间发生了冲突，我把/src路径下的文件重置之后，解决了这个问题。但是每次运行过程中都会出现模型加载不全的情况。这个在运行的时候没有报错，具体原因我也不知道为什么。

以下是运行结果：
/pcl_objects主题：

/pcl_table主题：

（4）集群对象

使用PCL的欧几里得聚类算法将其余点分割为单个对象。

为了执行欧几里得聚类，必须首先从点云cloud_objects构造一个kd树。

欧几里得聚类算法使用kd树数据结构来减少搜索相邻点的计算负担。尽管存在其他用于最近邻搜索的有效算法/数据结构，但PCL的Euclidian聚类算法仅支持kd树。

要构建kd树，首先需要将XYZRGB点云转换为XY点云，因为PCL的欧几里德聚类算法需要仅包含空间信息的点云。要创建这种无色的点云。我们将使用pc_helper.py中的函数。

接下来，从中构造一个kd树。为此，将以下代码添加到pcl_callback()节点中的函数：

# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()

构建完kd树后，可以执行以下集群提取：

# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold 
# as well as minimum and maximum cluster size (in points)
# NOTE: These are poor choices of clustering parameters
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.005)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(250)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()

cluster_indices现在包含每个群集的索引列表（列表列表）。

（5）集群可视化

我们已经执行了聚类步骤，并且具有每个对象的点列表（cluster_indices），现在我们要在RViz中可视化结果。

为每个细分对象选择唯一的颜色

为了在RViz中可视化结果，我们需要创建一个最终点云，我们将其称为PointCloud_PointXYZRGB类型的“cluster_cloud”。该云将包含每个分割对象的点，每组点具有唯一的颜色。

#Assign a color corresponding to each segmented object in scene
cluster_color = get_color_list(len(cluster_indices))

color_cluster_point_list = []

for j, indices in enumerate(cluster_indices):
    for i, indice in enumerate(indices):
        color_cluster_point_list.append([white_cloud[indice][0],
                                        white_cloud[indice][1],
                                        white_cloud[indice][2],
                                         rgb_to_float(cluster_color[j])])

#Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)

发布 ros_cluster_cloud

创建cluster_cloud后，现在可以将其转换为ROS的PointCloud2类型并发布。

 ros_cluster_cloud = pcl_to_ros(cluster_cloud)

在Rviz中可视化结果

要在RViz中查看分段的结果，需要创建一个新的发布者并将其发布为ros_cluster_cloud。

完成此操作后，保存并运行的节点，然后在RViz中只需将PointCloud2显示的主题下拉列表从屏幕快照更改为/sensor_stick/point_cloud即可。

完整程序如下：

#!/usr/bin/env python

# Import modules
from pcl_helper import *
import rospy
import pcl
from sklearn.cluster import DBSCAN
# TODO: Define functions as required

# Callback function for your Point Cloud Subscriber
def pcl_callback(pcl_msg):
    # TODO: Initialization
    # rospy.init_node('pcl_msg', anonymous=True)

    # TODO: Convert ROS msg to PCL data
    cloud = ros_to_pcl(pcl_msg)
    # TODO: Voxel Grid Downsampling
    vox = cloud.make_voxel_grid_filter()
    LEAF_SIZE = 0.02
    vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
    cloud = vox.filter()
    # TODO: PassThrough Filter
    passthrough = cloud.make_passthrough_filter()
    filter_axis = 'z'
    passthrough.set_filter_field_name(filter_axis)
    axis_min = 0.76
    axis_max = 1.3
    passthrough.set_filter_limits(axis_min, axis_max)
    cloud = passthrough.filter()
    # TODO: RANSAC Plane Segmentation
    seg = cloud.make_segmenter()
    seg.set_model_type(pcl.SACMODEL_PLANE)
    seg.set_method_type(pcl.SAC_RANSAC)
    max_distance = 0.01
    seg.set_distance_threshold(max_distance)
    inlier, coefficients = seg.segment()
    # TODO: Extract inliers and outliers
    cloud_table = cloud.extract(inlier, negative=False)
    cloud_objects = cloud.extract(inlier, negative=True)
    # TODO: Euclidean Clustering
    white_cloud = XYZRGB_to_XYZ(cloud_objects)
    tree = white_cloud.make_kdtree()
    ec = white_cloud.make_EuclideanClusterExtraction()
    ec.set_ClusterTolerance(0.05)
    ec.set_MinClusterSize(10)
    ec.set_MaxClusterSize(500)
    ec.set_SearchMethod(tree)
    cluster_indices = ec.Extract()
    # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
    # Assign a color corresponding to each segmented object in scene
    cluster_color = get_color_list(len(cluster_indices))

    color_cluster_point_list = []

    for j, indices in enumerate(cluster_indices):
        for i, indice in enumerate(indices):
            color_cluster_point_list.append([white_cloud[indice][0],
                                             white_cloud[indice][1],
                                             white_cloud[indice][2],
                                             rgb_to_float(cluster_color[j])])

    # Create new cloud containing all clusters, each with unique color
    cluster_cloud = pcl.PointCloud_PointXYZRGB()
    cluster_cloud.from_list(color_cluster_point_list)
    print(cluster_cloud)
    # TODO: Convert PCL data to ROS messages
    cloud_table_ros = pcl_to_ros(cloud_table)
    cloud_objects_ros = pcl_to_ros(cluster_cloud)
    # TODO: Publish ROS messages
    pcl_objects_pub.publish(cloud_objects_ros)
    pcl_table_pub.publish(cloud_table_ros)


if __name__ == '__main__':
    # TODO: ROS node initialization
    rospy.init_node('clustering', anonymous=True)
    # TODO: Create Subscribers
    pcl_sub = rospy.Subscriber('/sensor_stick/point_cloud', pc2.PointCloud2, pcl_callback, queue_size=1)
    # TODO: Create Publishers
    pcl_objects_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)
    pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)
    # Initialize color_list
    get_color_list.color_list = []

    # TODO: Spin while node is not shutdown
    while not rospy.is_shutdown():
        rospy.spin()

输出结果如下：

要想得到一个比较好的结果需要调整每个环节的参数，才能使得出的效果最好。

可以看到扫地机器人加载出来了，比较让我惊异的是，扫地机器人好像自己走动就挂到了桌子边上…