當前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

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

發(fā)布時間：2023/11/27 生活经验 26 豆豆

生活随笔收集整理的這篇文章主要介紹了 Udacity机器人软件工程师课程笔记（二十一) - 对点云进行集群可视化 - 聚类的分割 - K-means|K均值聚类, DBSCAN算法小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

聚類的分割

1.K-均值聚類

（1）K-均值聚類介紹

k均值聚類算法（k-means clustering algorithm）是一種迭代求解的聚類分析算法，其步驟是隨機選取K個對象作為初始的聚類中心，然后計算每個對象與各個種子聚類中心之間的距離，把每個對象分配給距離它最近的聚類中心。

聚類中心以及分配給它們的對象就代表一個聚類。每分配一個樣本，聚類的聚類中心會根據(jù)聚類中現(xiàn)有的對象被重新計算。這個過程將不斷重復直到滿足某個終止條件。終止條件可以是沒有（或最小數(shù)目）對象被重新分配給不同的聚類，沒有（或最小數(shù)目）聚類中心再發(fā)生變化，誤差平方和局部最小。

在具有不同k值的同一數(shù)據(jù)集上運行k均值聚類的示例（注意：在k = 50的圖中，顏色會重復出現(xiàn)，但表示單獨的聚類）。

（2）k - means算法

先隨機選取K個對象作為初始的聚類中心。然后計算每個對象與各個種子聚類中心之間的距離，把每個對象分配給距離它最近的聚類中心。聚類中心以及分配給它們的對象就代表一個聚類。一旦全部對象都被分配了，每個聚類的聚類中心會根據(jù)聚類中現(xiàn)有的對象被重新計算。這個過程將不斷重復直到滿足某個終止條件。終止條件可以是以下任何一個：
1)沒有（或最小數(shù)目）對象被重新分配給不同的聚類。
2)沒有（或最小數(shù)目）聚類中心再發(fā)生變化。
3)誤差平方和局部最小。

偽代碼：
假設我們有 $n$ 個數(shù)據(jù)點，要將其分成 $k$ 類。

先隨機選取 $k$ 個點 $c_1, c_2, ..., c_k$ 作為初始的聚類中心。
定義收斂/終止標準(解決方案的穩(wěn)定性和最大迭代次數(shù))
while 不符合收斂/終止準則 do:
1. for $i = 1$ to $n$ :
  計算 $p_i$ 到每個簇中心的距離
  分配 $p_i$ 到最接近它的質心，并相應地標記它
  endfor
2. for $j = 1$ to $k$ :
  根據(jù)所有數(shù)據(jù)點的平均值重新計算 $j$ 的質心
  endfor
  endwhile

（3）k -means 代碼

首先給出完整的代碼：

import numpy as np
import matplotlib.pyplot as plt
import cv2# 定義一個函數(shù)來生成集群
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 = 要生成的集群數(shù)量# pts_minmax = 每個集群的點數(shù)范圍# x_mult = 乘法器的范圍，在x方向修改集群的大小# y_mult = 乘法器的范圍，在y方向修改集群的大小# x_off = 簇在x方向上的位置偏移范圍# y_off = 簇在y方向上的位置偏移范圍# 初始化一些空列表以接收集群成員位置clusters_x = []clusters_y = []# 在給定的參數(shù)范圍內(nèi)生成隨機值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)# 生成隨機集群給定參數(shù)值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格式的單個數(shù)據(jù)集
data = np.float32((np.concatenate(clusters_x), np.concatenate(clusters_y))).transpose()# k - means參數(shù)定義
# 要定義的集群數(shù)量
k_clusters = 7
# 要執(zhí)行的最大迭代數(shù)
max_iter = 10
# 停止迭代的精度準則
epsilon = 1.0
# 以OpenCV格式定義標準
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
# 調(diào)用數(shù)據(jù)集上的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提供了了一個沙箱環(huán)境，我們可以在其中測試k-means算法的局限。

# 定義一個函數(shù)來生成集群
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 = 要生成的集群數(shù)量# pts_minmax = 每個集群的點數(shù)范圍# x_mult = 乘法器的范圍，在x方向修改集群的大小# y_mult = 乘法器的范圍，在y方向修改集群的大小# x_off = 簇在x方向上的位置偏移范圍# y_off = 簇在y方向上的位置偏移范圍# 初始化一些空列表以接收集群成員位置clusters_x = []clusters_y = []# 在給定的參數(shù)范圍內(nèi)生成隨機值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)# 生成隨機集群給定參數(shù)值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

首先要做的是生成一些集群。現(xiàn)在提供了一個名為cluster_gen()的函數(shù)，它將從隨機高斯分布生成簡單的集群總體。

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

函數(shù)的作用是，返回一個隨機整型數(shù)，范圍從低（包括）到高（不包括），即[low, high)。

如果沒有寫參數(shù)high的值，則返回[0,low)的值。

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

當函數(shù)括號內(nèi)沒有參數(shù)時，則返回一個浮點數(shù)；
當函數(shù)括號內(nèi)有一個參數(shù)時，則返回秩為1的數(shù)組，不能表示向量和矩陣；
當函數(shù)括號內(nèi)有兩個及以上參數(shù)時，則返回對應維度的數(shù)組，能表示向量或矩陣；

生成集群后，需要將數(shù)據(jù)轉換為OpenCV所需要的格式，然后定義k-means算法的約束，最后調(diào)用該cv2.kmeans()函數(shù)。

# 生成一些集群
n_clusters = 7
clusters_x, clusters_y = cluster_gen(n_clusters)
# 轉換為OpenCV格式的單個數(shù)據(jù)集
data = np.float32((np.concatenate(clusters_x), np.concatenate(clusters_y))).transpose()# k - means參數(shù)定義
# 要定義的集群數(shù)量
k_clusters = 7
# 要執(zhí)行的最大迭代數(shù)
max_iter = 10
# 停止迭代的精度準則
epsilon = 1.0
# 以OpenCV格式定義標準
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, max_iter, epsilon)
# 調(diào)用數(shù)據(jù)集上的k-means算法
compactness, label, center = cv2.kmeans(data, k_clusters, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS)

關于opencv下的kmean算法，函數(shù)為cv2.kmeans():

函數(shù)的格式為：kmeans(data, K, bestLabels, criteria, attempts, flags)

data: 分類數(shù)據(jù)。
K: 分類數(shù)，opencv2的kmeans分類是需要已知分類數(shù)的。
bestLabels：預設的分類標簽：沒有的話None
criteria：迭代停止的模式選擇，這是一個含有三個元素的元組型數(shù)。格式為（type,max_iter,epsilon）
其中，type又有兩種選擇：
cv2.TERM_CRITERIA_EPS:精確度（誤差）滿足epsilon停止。
cv2.TERM_CRITERIA_MAX_ITER：迭代次數(shù)超過max_iter停止。
cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER，兩者合體，任意一個滿足結束。
attempts：重復試驗kmeans算法次數(shù)，將會返回最好的一次結果
flags：初始類中心選擇，兩種方法：cv2.KMEANS_PP_CENTERS; cv2.KMEANS_RANDOM_CENTERS

程序輸出如下:

2. DBSCAN算法

（1）DBSCAN算法介紹

左側是原始數(shù)據(jù)，右側是由DBSCAN算法識別的聚類。對于DBSCAN集群，大色點表示核心集群成員，小色點表示集群邊緣成員，小黑點表示離群值。

DBSCAN代表帶噪聲的應用程序的基于密度的空間聚類。當我們不知道數(shù)據(jù)中預期有多少個聚類時，該算法是k均值的一種很好的替代方法，但是我們需要知道有關點應如何按照密度（聚類中點之間的距離）進行聚類的知識。

DBSCAN算法通過對在某個閾值距離內(nèi)的數(shù)據(jù)點進行分組來創(chuàng)建聚類 $d_ {th}$ 從數(shù)據(jù)中最近的其他點開始。
該算法有時也稱為“歐幾里得聚類”，因為是否在特定聚類中放置點的決定是基于該點與其他聚類成員之間的“歐幾里得距離”。

歐幾里得距離是連接兩個點的直線的長度，但是定義數(shù)據(jù)中點位置的坐標不必是空間坐標。例如，可以在顏色空間或數(shù)據(jù)的任何其他特征空間中定義它們。

點之間的歐幾里得距離 $p\bold {p}$ 和 $q\bold{q}$ 在n維數(shù)據(jù)集中，其中的位置 $p\bold {p}$ 由坐標定義 $p_1，p_2，…，p_n）$ 和位置 $q\bold{q}$ 定義為 $q_1，q_2，…，q_n）$ ，那么兩點之間的距離就是：

$\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$ 的數(shù)據(jù)集 $P$ :

為組成集群的最小點數(shù)設置約束(min_samples)
設置距離閾值或聚類點之間的最大距離(max_dist)
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

上面的代碼做了一些簡化，以幫助理解算法。如果隨機訪問每個數(shù)據(jù)點，最終將創(chuàng)建比所需數(shù)量更多的群集，并且必須以某種方式合并它們。我們可以通過控制訪問數(shù)據(jù)點的順序來避免這種情況。

使用我們的第一個合格數(shù)據(jù)點創(chuàng)建新集群后，我們會將其所有鄰居添加到集群中。然后，我們將添加它的鄰居的鄰居以及他們的鄰居，直到我們訪問了屬于該集群的每個數(shù)據(jù)點。只有這樣做之后，我們才能繼續(xù)隨機選擇另一個數(shù)據(jù)點。這樣，我們可以保證集群是完整的，并且數(shù)據(jù)集中不再有屬于該集群的點。

它的工作原理如下：首先我們選擇兩個參數(shù)，正數(shù)epsilon和自然數(shù)minPoints。然后，我們從數(shù)據(jù)集中選擇任意點開始。如果距epsilon的距離內(nèi)的minPoints個點以上（包括原始點本身），則我們將所有這些點都視為“群集”的一部分。然后，我們通過檢查所有新點并查看它們在epsilon距離內(nèi)是否也擁有超過minPoints點來擴展該群集，如果是，則以遞歸方式擴展該群集。

最終，我們用完了要添加到群集中的點。然后，我們選擇一個新的任意點并重復該過程。現(xiàn)在，完全有可能我們選擇的一個點在其epsilon球中少于minPoints，并且也不屬于任何其他群集。如果是這種情況，則將其視為不屬于任何群集的“噪聲點”。

DBSCAN算法的完整偽代碼實現(xiàn)可以在DBSCAN Wikipedia頁面上找到。

對于其中min_samples= 4 的示例，以下是結果的外觀外觀：

藍點是在max_dist內(nèi)至少有三個鄰居的核心集群成員（滿足鄰居數(shù)+自身>= min_samples)。黃點是有鄰居的邊緣成員，但少于三個。黑點是沒有鄰居的異常值。

（3）相關代碼

import numpy as np
import matplotlib.pyplot as plt
import cv2
from sklearn.cluster import DBSCAN# 定義一個函數(shù)來生成集群
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 = 要生成的集群數(shù)量# pts_minmax = 每個集群的點數(shù)范圍# x_mult = 乘法器的范圍，在x方向修改集群的大小# y_mult = 乘法器的范圍，在y方向修改集群的大小# x_off = 簇在x方向上的位置偏移范圍# y_off = 簇在y方向上的位置偏移范圍# 初始化一些空列表以接收集群成員位置clusters_x = []clusters_y = []# 在給定的參數(shù)范圍內(nèi)生成隨機值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)# 生成隨機集群給定參數(shù)值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格式的單個數(shù)據(jù)集
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）環(huán)境搭建

現(xiàn)在是時候回到ROS，Gazebo和Rviz對點云進行一些聚類分割了。在本練習中，我們將使用PCL庫函數(shù)EuclideanClusterExtraction()，該函數(shù)在3D點云上執(zhí)行DBSCAN集群搜索。

在本練習中，我們將使用上一課中的相同桌面場景，我們已經(jīng)在虛擬機（或本地）上設置了Catkin工作區(qū)，則第一步是將/sensor_stick目錄及其所有內(nèi)容復制或移動到~/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重新建立了一遍依賴關系。然后重新執(zhí)行了操作。在新的Terminator下執(zhí)行了3次roslaunch之后得到了正常顯示，但是依舊報了很多錯。猜測有可能是src路徑下的依賴沖突了。

以下為正常顯示：

我們在這個練習中的目標是編寫一個ROS節(jié)點，將相機數(shù)據(jù)作為點云，過濾該點云，然后使用歐幾里得聚類對各個對象進行分類。

我們將通過添加代碼來說編寫節(jié)點，將點云數(shù)據(jù)作為信息發(fā)布到名為/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 messagesif __name__ == '__main__':# TODO: ROS node initialization# TODO: Create Subscribers# TODO: Create Publishers# Initialize color_listget_color_list.color_list = []# TODO: Spin while node is not shutdown

（2）發(fā)布點云

接下來進行一個對發(fā)布點云的ros節(jié)點進行構建，程序如下

#!/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 messagespcl_objects_pub.publish(pcl_msg)pcl_table_pub.publish(pcl_msg)if __name__ == '__main__':# TODO: ROS node initializationrospy.init_node('clustering', anonymous=True)# TODO: Create Subscriberspcl_sub = rospy.Subscriber('/sensor_stick/point_cloud', pc2.PointCloud2, pcl_callback, queue_size=1)# TODO: Create Publisherspcl_object_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)# Initialize color_listget_color_list.color_list = []# TODO: Spin while node is not shutdownwhile not rospy.is_shutdown():rospy.spin()

（3）過濾和RANSAC

接下來應用過濾并執(zhí)行RANSAC平面分割。以下是需要采取的步驟：

通過應用體素網(wǎng)格過濾器對點云進行下采樣。
應用通過過濾器以隔離桌子和對象。
執(zhí)行RANSAC平面擬合以識別桌子。
使用ExtractIndices過濾器來創(chuàng)建包含桌子和對象分開新的點云（將它們命名為cloud_table和cloud_objects）。

完成這些操作后，將這些新的云轉換回ROS消息格式，并將它們發(fā)布到先前創(chuàng)建的主題中，/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 datacloud = ros_to_pcl(pcl_msg)# TODO: Voxel Grid Downsamplingvox = cloud.make_voxel_grid_filter()LEAF_SIZE = 0.01vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)cloud = vox.filter()# TODO: PassThrough Filterpassthrough = cloud.make_passthrough_filter()filter_axis = 'z'passthrough.set_filter_field_name(filter_axis)axis_min = 0.76axis_max = 1.3passthrough.set_filter_limits(axis_min, axis_max)cloud = passthrough.filter()# TODO: RANSAC Plane Segmentationseg = cloud.make_segmenter()seg.set_model_type(pcl.SACMODEL_PLANE)seg.set_method_type(pcl.SAC_RANSAC)max_distance = 0.01seg.set_distance_threshold(max_distance)inlier, coefficients = seg.segment()# TODO: Extract inliers and outlierscloud_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 messagescloud_table_ros = pcl_to_ros(cloud_table)cloud_objects_ros = pcl_to_ros(cloud_objects)# TODO: Publish ROS messagespcl_objects_pub.publish(cloud_objects_ros)pcl_table_pub.publish(cloud_table_ros)if __name__ == '__main__':# TODO: ROS node initializationrospy.init_node('clustering', anonymous=True)# TODO: Create Subscriberspcl_sub = rospy.Subscriber('/sensor_stick/point_cloud', pc2.PointCloud2, pcl_callback, queue_size=1)# TODO: Create Publisherspcl_objects_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)# Initialize color_listget_color_list.color_list = []# TODO: Spin while node is not shutdownwhile not rospy.is_shutdown():rospy.spin()

在這個完成過程中發(fā)生了一個比較嚴重的bug，就是我的命名空間發(fā)生了沖突，我把/src路徑下的文件重置之后，解決了這個問題。但是每次運行過程中都會出現(xiàn)模型加載不全的情況。這個在運行的時候沒有報錯，具體原因我也不知道為什么。

以下是運行結果：
/pcl_objects主題：

/pcl_table主題：

（4）集群對象

使用PCL的歐幾里得聚類算法將其余點分割為單個對象。

為了執(zhí)行歐幾里得聚類，必須首先從點云cloud_objects構造一個kd樹。

歐幾里得聚類算法使用kd樹數(shù)據(jù)結構來減少搜索相鄰點的計算負擔。盡管存在其他用于最近鄰搜索的有效算法/數(shù)據(jù)結構，但PCL的Euclidian聚類算法僅支持kd樹。

要構建kd樹，首先需要將XYZRGB點云轉換為XY點云，因為PCL的歐幾里德聚類算法需要僅包含空間信息的點云。要創(chuàng)建這種無色的點云。我們將使用pc_helper.py中的函數(shù)。

接下來，從中構造一個kd樹。為此，將以下代碼添加到pcl_callback()節(jié)點中的函數(shù)：

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

構建完kd樹后，可以執(zhí)行以下集群提取：

# 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現(xiàn)在包含每個群集的索引列表（列表列表）。

（5）集群可視化

我們已經(jīng)執(zhí)行了聚類步驟，并且具有每個對象的點列表（cluster_indices），現(xiàn)在我們要在RViz中可視化結果。

為每個細分對象選擇唯一的顏色

為了在RViz中可視化結果，我們需要創(chuàng)建一個最終點云，我們將其稱為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)

發(fā)布 ros_cluster_cloud

創(chuàng)建cluster_cloud后，現(xiàn)在可以將其轉換為ROS的PointCloud2類型并發(fā)布。

 ros_cluster_cloud = pcl_to_ros(cluster_cloud)

在Rviz中可視化結果

要在RViz中查看分段的結果，需要創(chuàng)建一個新的發(fā)布者并將其發(fā)布為ros_cluster_cloud。

完成此操作后，保存并運行的節(jié)點，然后在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 datacloud = ros_to_pcl(pcl_msg)# TODO: Voxel Grid Downsamplingvox = cloud.make_voxel_grid_filter()LEAF_SIZE = 0.02vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)cloud = vox.filter()# TODO: PassThrough Filterpassthrough = cloud.make_passthrough_filter()filter_axis = 'z'passthrough.set_filter_field_name(filter_axis)axis_min = 0.76axis_max = 1.3passthrough.set_filter_limits(axis_min, axis_max)cloud = passthrough.filter()# TODO: RANSAC Plane Segmentationseg = cloud.make_segmenter()seg.set_model_type(pcl.SACMODEL_PLANE)seg.set_method_type(pcl.SAC_RANSAC)max_distance = 0.01seg.set_distance_threshold(max_distance)inlier, coefficients = seg.segment()# TODO: Extract inliers and outlierscloud_table = cloud.extract(inlier, negative=False)cloud_objects = cloud.extract(inlier, negative=True)# TODO: Euclidean Clusteringwhite_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 scenecluster_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 colorcluster_cloud = pcl.PointCloud_PointXYZRGB()cluster_cloud.from_list(color_cluster_point_list)print(cluster_cloud)# TODO: Convert PCL data to ROS messagescloud_table_ros = pcl_to_ros(cloud_table)cloud_objects_ros = pcl_to_ros(cluster_cloud)# TODO: Publish ROS messagespcl_objects_pub.publish(cloud_objects_ros)pcl_table_pub.publish(cloud_table_ros)if __name__ == '__main__':# TODO: ROS node initializationrospy.init_node('clustering', anonymous=True)# TODO: Create Subscriberspcl_sub = rospy.Subscriber('/sensor_stick/point_cloud', pc2.PointCloud2, pcl_callback, queue_size=1)# TODO: Create Publisherspcl_objects_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)# Initialize color_listget_color_list.color_list = []# TODO: Spin while node is not shutdownwhile not rospy.is_shutdown():rospy.spin()

輸出結果如下：

要想得到一個比較好的結果需要調(diào)整每個環(huán)節(jié)的參數(shù)，才能使得出的效果最好。

可以看到掃地機器人加載出來了，比較讓我驚異的是，掃地機器人好像自己走動就掛到了桌子邊上…

總結

以上是生活随笔為你收集整理的Udacity机器人软件工程师课程笔记（二十一) - 对点云进行集群可视化 - 聚类的分割 - K-means|K均值聚类, DBSCAN算法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

上一篇： Udacity机器人软件工程师课程笔记（
下一篇： Udacity机器人软件工程师课程笔记（