物體識別

1.HSV色彩空間

如果要進行顏色檢測，HSV顏色空間是當前最常用的。

HSV(Hue, Saturation, Value)是根據顏色的直觀特性由A. R. Smith在1978年創建的一種顏色空間, 也稱六角錐體模型(Hexcone Model)。這個模型中顏色的參數分別是：色調（H），飽和度（S），亮度（V）。

HSV模型的三維表示從RGB立方體演化而來。設想從RGB沿立方體對角線的白色頂點向黑色頂點觀察，就可以看到立方體的六邊形外形。六邊形邊界表示色彩，水平軸表示純度，明度沿垂直軸測量。

2.顏色直方圖

使用生成的點云，將需要為3D空間中找到的點構造顏色直方圖。但是，出于示例練習的目的，對2D圖像中的像素進行操作就足夠了。

首先我們先輸出RGB圖像的直方圖，程序如下:

import matplotlib.image as mpimg
import matplotlib.pyplot as plt
import numpy as np# 載入圖片
image = mpimg.imread('hsv_image.jpg')# 取R, G, B的直方圖
r_hist = np.histogram(image[:, :, 0], bins=32, range=(0, 256))
g_hist = np.histogram(image[:, :, 1], bins=32, range=(0, 256))
b_hist = np.histogram(image[:, :, 2], bins=32, range=(0, 256))# 創建 bin 中心
print(r_hist)
bin_edges = r_hist[1]
bin_centers = (bin_edges[1:] + bin_edges[0:len(bin_edges)-1]) / 2# 繪制直方圖
fig = plt.figure(figsize=(12, 3))
plt.subplot(131)
plt.bar(bin_centers, r_hist[0])
plt.xlim(0, 256)
plt.title('R Histogram')
plt.subplot(132)
plt.bar(bin_centers, g_hist[0])
plt.xlim(0, 256)
plt.title('G Histogram')
plt.subplot(133)
plt.bar(bin_centers, b_hist[0])
plt.xlim(0, 256)
plt.title('B Histogram')
plt.show() 

直方圖輸出如下：

使用的圖片為：

繪制HSV直方圖，程序如下：

import matplotlib.image as mpimg
import matplotlib.pyplot as plt
import numpy as np
import cv2def color_hist(img, nbins=32, bins_range=(0, 256)):img_hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)h_hist = np.histogram(img_hsv[:, :, 0], bins=nbins, range=bins_range)s_hist = np.histogram(img_hsv[:, :, 1], bins=nbins, range=bins_range)v_hist = np.histogram(img_hsv[:, :, 2], bins=nbins, range=bins_range)# 轉換為浮點數，保證在下一步不進行整數除法hist_features = np.concatenate((h_hist[0], s_hist[0], v_hist[0])).astype(np.float64)# 對結果歸一化，使直方圖中所有bin的總和為1norm_features = hist_features / np.sum(hist_features)return norm_features# 載入圖片
image = mpimg.imread('hsv_image.jpg')
feature_vec = color_hist(image)
plt.imshow(image)if feature_vec is not None:fig = plt.figure(figsize=(12, 6))plt.plot(feature_vec)plt.title('HSV Feature Vector', fontsize=30)plt.tick_params(axis='both', which='major', labelsize=20)fig.tight_layout()plt.show()
else:print('Your function is returing None..')

輸出如下：

3.支持向量機SVM

支持向量機或“ SVM”只是一種特殊的受監督機器學習算法的名稱，它可以將數據集的參數空間表征為離散類。

SVM通過將迭代方法應用于訓練數據集來工作，其中訓練集中的每個項目都由特征向量和標簽來表征。在上圖中，每個點僅由兩個特征（A和B）表征。每個點的顏色與其標簽相對應，或者與其在數據集中表示的對象類別相對應。

將SVM應用于此訓練集可將/整個參數空間表征為離散的類。參數空間中類之間的劃分稱為“決策邊界”，在這里由覆蓋在數據上的彩色多邊形表示。創建決策邊界意味著考慮具有功能但沒有標簽的新對象時，可以立即將其分配給特定的類。換句話說，一旦對SVM進行了訓練，就可以將其用于對象識別。

Scikit-Learn中的SVM

sklearnPython中的Scikit-Learn或軟件包提供了多種SVM實現。為了達到我們的目的，我們將使用帶有線性內核的基本SVM，因為它往往在分類方面做得很好，并且比更復雜的實現運行得更快，但是有必要查看sklearn.svm軟件包中的其他可能性。

訓練數據

在訓練SVM之前，我們需要一個標記數據集。為了快速生成一些數據，我們將使用cluster_gen()功能，我們在前面定義的教訓聚類市場細分。但是，現在，我們將為每個群集數據點以及x和y位置提供函數輸出標簽

n_clusters = 5
clusters_x, clusters_y, labels = cluster_gen(n_clusters)

在這種情況下，特征是聚類點的x和y位置，標簽只是與每個聚類關聯的數字。要將它們用作訓練數據，需要轉換為sklearn.svm.SVC（）期望的格式，它是形狀(n_samples, m_features)和長度標簽的功能集n_samples（在這種情況下，n_samples是聚類點的總數，m_features為2 ）。在機器學習應用程序中，通常會調用功能集X和標簽y。

根據cluster_gen()的輸出格式，可以創建如下特性和標簽:

import numpy as np
X = np.float32((np.concatenate(clusters_x), np.concatenate(clusters_y))).transpose()
y = np.float32((np.concatenate(labels)))

整理好訓練數據后，sklearn就可以輕松創建和訓練SVM！

from sklearn import svm
svc = svm.SVC(kernel='linear').fit(X, y)

在下面的程序中，可以更改數據集。可以在np.random.seed(424)語句中更改數字以生成其他數據集。可以查看sklearn.svm.SVC（）的文檔，以查看可以調整的參數以及結果如何變化。

import numpy as np
import matplotlib.pyplot as plt
from sklearn import svm# 定義一個函數來生成集群
def cluster_gen(n_clusters, pts_minmax=(100, 500), x_mult=(2, 7), y_mult=(2, 7),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 = []labels = []# 生成隨機值給定參數范圍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)labels.append(np.zeros_like(xpts) + idx)# 返回集群位置和標簽return clusters_x, clusters_y, labelsnp.random.seed(424)  # 更改編號以生成不同的集群n_clusters = 3
clusters_x, clusters_y, labels = cluster_gen(n_clusters)# 轉換為sklearn格式的培訓數據集
X = np.float32((np.concatenate(clusters_x), np.concatenate(clusters_y))).transpose()
y = np.float32((np.concatenate(labels)))# 創建一個SVM實例，并對數據進行擬合。
ker = 'linear'
svc = svm.SVC(kernel=ker).fit(X, y)# 創建一個網格，我們將使用彩色來確定表面
# Plotting Routine courtesy of: http://scikit-learn.org/stable/auto_examples/svm/plot_iris.html#sphx-glr-auto-examples-svm-plot-iris-py
# 注意:這種配色方案在> 7個簇或更多的地方失效h = 0.2  # 在網格中的步長
x_min, x_max = X[:, 0].min() - 1, X[:, 0].max() + 1  # -1 and +1 to add some margins
y_min, y_max = X[:, 1].min() - 1, X[:, 1].max() + 1
xx, yy = np.meshgrid(np.arange(x_min, x_max, h),np.arange(y_min, y_max, h))# 對網格的每個塊進行分類(用于分配其顏色)
Z = svc.predict(np.c_[xx.ravel(), yy.ravel()])# 將結果放入顏色圖中
Z = Z.reshape(xx.shape)
plt.contourf(xx, yy, Z, cmap=plt.cm.coolwarm, alpha=0.8)# 繪制訓練點
plt.scatter(X[:, 0], X[:, 1], c=y, cmap=plt.cm.coolwarm, edgecolors='black')
plt.xlim(xx.min(), xx.max())
plt.ylim(yy.min(), yy.max())
plt.xticks(())
plt.yticks(())
plt.title('SVC with '+ker+' kernel', fontsize=20)
plt.show()

輸出如下：

4.SVM圖像分類

我們在，我們已經了解了如何使用SVM對多類數據集進行分類，但是只有兩個功能描述了每個元素。有了點云數據，w將擁有一個豐富的功能集，其中包含顏色和表面法線直方圖。具有豐富功能集的分類與具有兩個功能的分類工作相同，但更難以可視化，因此我們將通過使用顏色直方圖的圖像分類示例進行學習。

為了演示圖像分類，我們將借鑒自動駕駛汽車納米學位計劃的一項練習。在本練習中，數據集由數百個汽車圖像以及可能在汽車場景中發現的其他圖像組成，但還有其他一些。我們的目標是訓練SVM根據由顏色直方圖組成的輸入特征向量來識別圖像是否包含汽車。在這里，我們將介紹一些與準備訓練數據和評估分類器性能有關的概念。

首先，我們會在汽車圖像和非汽車圖像中為每個圖像提取顏色特征，然后將特征向量縮放為零均值和單位方差。

之后，我們將定義標簽向量，將數據洗牌并將其拆分為訓練和測試集，最后，定義一個分類器并對其進行訓練。

這種情況下，標簽向量將只是一個二進制向量，指示數據集中的每個特征向量是對應于汽車還是非汽車（汽車為1，非汽車為0）。在這里，我們有一個稱為extract_features()的函數，該color_hist()函數將調用在上一個練習中定義的函數，并從圖像數據集中生成一系列特征。

# Define a function to extract features from a list of images
# Have this function call color_hist()
def extract_features(imgs, hist_bins=32, hist_range=(0, 256)):# Create a list to append feature vectors tofeatures = []# Iterate through the list of imagesfor file in imgs:# Read in each one by oneimage = mpimg.imread(file)# Apply color_hist() hist_features = color_hist(image, nbins=hist_bins, bins_range=hist_range)# Append the new feature vector to the features listfeatures.append(hist_features)# Return list of feature vectorsreturn features

給定汽車和非汽車特征的列表，我們可以定義標簽矢量（只是一堆的一和零），如下所示：

import numpy as np
# Define a labels vector based on features lists
y = np.hstack((np.ones(len(car_features)), np.zeros(len(notcar_features))))

接下來，我們將疊加和縮放我們的特征向量。堆疊成一個單獨的數組是為了得到sklearn所期望的格式。擴展是一個更微妙的問題。在堆疊的陣列中，每個要素將占據一列。當某些功能的大小遠遠大于其他功能時，可能會導致分類器的性能下降。因此，執行每列歸一化以確保所有特征大致相同的比例（在這里，我們將平均值和單位方差縮放為零）始終是一個好方法。

from sklearn.preprocessing import StandardScaler
# Create an array stack of feature vectors
X = np.vstack((car_features, notcar_features)).astype(np.float64)
# Fit a per-column scaler
X_scaler = StandardScaler().fit(X)
# Apply the scaler to X
scaled_X = X_scaler.transform(X)

現在我們準備好將數據洗牌并將其分為訓練和測試集。在單獨的數據集上測試分類器總是一個好主意，但是首先應該隨機處理數據。這確保了數據的任何排序(例如，數據集開頭的一堆紅色汽車和結尾的藍色汽車)都不會影響分類器的訓練。

為此，我們將使用Scikit-Learn train_test_split()函數，但值得注意的是，該函數最近從sklearn.cross_validation軟件包（sklearn版本== 0.17）移動到sklearn.model_selection軟件包（sklearn版本> = 0.18）。

在測驗編輯器中，我們仍在運行sklearnv0.17，因此我們將其導入為：

from sklearn.cross_validation import train_test_split
# But, if you are using scikit-learn >= 0.18 then use this:
# from sklearn.model_selection import train_test_split

train_test_split()執行數據的隨機播放和拆分，可以這樣稱呼它（此處選擇每次使用不同的隨機狀態初始化隨機播放）：

# Split up data into randomized training and test sets
rand_state = np.random.randint(0, 100)
X_train, X_test, y_train, y_test = train_test_split(scaled_X, y, test_size=0.2, random_state=rand_state)

現在，現在就可以定義和訓練分類器了。在這里，我們將對線性內核使用相同的SVC。要定義和訓練分類器，只需幾行代碼：

from sklearn.svm import LinearSVC
# Use a linear SVC (support vector classifier)
svc = SVC(kernel='linear')
# Train the SVC
svc.fit(X_train, y_train)

然后，可以像這樣檢查測試數據集上分類器的準確性：

print('Test Accuracy of SVC = ', svc.score(X_test, y_test))

或者，可以對測試數據的一部分進行預測，然后直接與基本事實進行比較：

print('My SVC predicts: ', svc.predict(X_test[0:10].reshape(1, -1)))
print('For labels: ', y_test[0:10])

histbin在下面的練習中使用參數值，以查看分類器準確性和訓練時間如何隨特征向量輸入而變化。

完整程序如下：

import matplotlib.image as mpimg
import matplotlib.pyplot as plt
import numpy as np
import cv2
import glob
import time
from sklearn.svm import SVC
from sklearn.preprocessing import StandardScaler
# NOTE: the next import is only valid
# for scikit-learn version <= 0.17
# if you are using scikit-learn >= 0.18 then use this:
from sklearn.model_selection import train_test_split
# from sklearn.cross_validation import train_test_split# 定義一個函數來計算顏色直方圖特征,輸入為圖片，返回特征向量
def color_hist(img, nbins=32, bins_range=(0, 256)):# 將RGB轉換為HSVhsv_img = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)# 計算各個HSV通道的直方圖h_hist = np.histogram(hsv_img[:, :, 0], bins=nbins, range=bins_range)s_hist = np.histogram(hsv_img[:, :, 1], bins=nbins, range=bins_range)v_hist = np.histogram(hsv_img[:, :, 2], bins=nbins, range=bins_range)# 將直方圖連接成單個特征向量hist_features = np.concatenate((h_hist[0], s_hist[0], v_hist[0])).astype(np.float64)# 歸一化norm_features = hist_features / np.sum(hist_features)# 返回特征向量return norm_features# 定義一個從圖像列表中提取特征的函數，輸入為圖片列表，返回特征向量列表
# 這個函數調用color_hist()
def extract_features(imgs, hist_bins=32, hist_range=(0, 256)):# 創建一個列表來附加特征向量features = []# 遍歷圖像列表for file in imgs:image = mpimg.imread(file)# 應用color_hist ()hist_features = color_hist(image, nbins=hist_bins, bins_range=hist_range)# 將新的特征向量附加到特征列表中features.append(hist_features)# 返回特征向量列表return features# 讀取汽車和非汽車圖像
images = glob.glob('*/*/*/*.jpeg')
cars = []
notcars = []for image in images:if 'image' in image or 'extra' in image:notcars.append(image)else:cars.append(image)if cars == [] or notcars == []:print("images is empty! please check your images path!")# TODO play with this value to see how your classifier
# 在不同的場景下執行
histbin = 32
# 讀取汽車和非汽車圖像列表，應用extract_features函數
car_features = extract_features(cars, hist_bins=histbin, hist_range=(0, 256))
notcar_features = extract_features(notcars, hist_bins=histbin, hist_range=(0, 256))# 創建一個特征向量數組堆疊
X = np.vstack((car_features, notcar_features)).astype(np.float64)
# 確定每列的定標器
X_scaler = StandardScaler().fit(X)
# 把定標器應用到X軸上
scaled_X = X_scaler.transform(X)# 定義標簽向量
y = np.hstack((np.ones(len(car_features)), np.zeros(len(notcar_features))))# 將數據分成隨機的訓練和測試集
rand_state = np.random.randint(0, 100)
X_train, X_test, y_train, y_test = train_test_split(scaled_X, y, test_size=0.2, random_state=rand_state)print('Dataset includes', len(cars), 'cars and', len(notcars), 'not-cars')
print('Using', histbin, 'histogram bins')
print('Feature vector length:', len(X_train[0]))
# 使用線性SVC
svc = SVC(kernel='linear')
# SVC的訓練時間
t = time.time()
svc.fit(X_train, y_train)
t2 = time.time()
print(round(t2-t, 2), 'Seconds to train SVC...')
# SVC的分數
print('Test Accuracy of SVC = ', round(svc.score(X_test, y_test), 4))
# 單個樣本的預測時間
t = time.time()
n_predict = 10
print('My SVC predicts: ', svc.predict(X_test[0:n_predict]))
print('For these', n_predict, 'labels: ', y_test[0:n_predict])
t2 = time.time()
print(round(t2-t, 5), 'Seconds to predict', n_predict,'labels with SVC')

程序輸出為：

Dataset includes 1196 cars and 1125 not-cars
Using 32 histogram bins
Feature vector length: 96
0.07 Seconds to train SVC...
Test Accuracy of SVC =  0.9914
My SVC predicts:  [1. 1. 1. 1. 1. 0. 1. 0. 0. 0.]
For these 10 labels:  [1. 1. 1. 1. 1. 0. 1. 0. 0. 0.]
0.00099 Seconds to predict 10 labels with SVC

5.識別實例

這個練習提供了一個非常簡單的gazebo世界，我們可以從練習1和練習2中從點云中分割的對象中提取顏色和形狀特征，以便訓練分類器來檢測它們。

本練習的目標是訓練SVM識別場景中的特定對象。為此，首先要提取一組特征和標簽，然后訓練SVM分類器，最后使用分類器預測分節點云中的對象。

如果已經克隆了robond感知練習庫，那么需要做的就是再次獲取git pull以獲得練習3的代碼。

ps:這個練習需要ROS，所以需要在Udacity提供的VM中或在自己的本地Linux/ROS安裝上完成這些步驟。

環境配置

如果完成了練習1和練習2，那么在~/catkin_ws/src目錄中已經有一個sensor_stick文件夾了。應該首先復制為練習cp2編寫的Python腳本(segmentation .py)，然后將舊的sensor_stick文件夾替換為存儲庫中包含在練習3目錄中的新sensor_stick文件夾。

如果還沒有sensor_stick目錄，那么首先復制/移動sensor_stick文件夾到活動ros工作區的~/catkin_ws/src目錄。

從練習3目錄:

cp -r sensor_stick/ ~/catkin_ws/src/

通過使用該rosdep install工具并運行，確保已解決所有依賴項

catkin_make：
$ cd ~/catkin_ws
$ rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y
$ catkin_make

如果它們還不在其中，將以下行添加到.bashrc文件中

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

產生特征

要開始生成功能，啟動training.launch文件以啟動Gazebo環境。一個空的環境應該只在場景中出現帶RGBD相機的棒狀結構：

$ cd ~/catkin_ws
$ roslaunch sensor_stick training.launch

注意終端中的錯誤，如果涼亭崩潰或沒有出現，再可以嘗試一次，有時需要嘗試幾次。

ps:看來之前的出錯的原因有可能和程序本身的bug有關系

捕捉功能

接下來，打開一個新的終端，運行capture_features.py腳本以捕獲并保存環境中每個對象的功能。該腳本以隨機方向生成每個對象（每個對象默認5個方向），并根據每個隨機方向產生的點云計算特征。

$ cd ~/catkin_ws
$ rosrun sensor_stick capture_features.py

可以看到對象正在在Gazebo生成。每個隨機方向需要5-10秒(取決于機器的資源)。總共有7個對象，所以需要一段時間才能完成。當它運行結束時，應該有一個包含數據集的特性和標簽的 training_set.sav 文件。
注意: training_set.sav 文件將保存在的catkin_ws文件夾中。

訓練SVM

一旦特征提取成功完成，就可以訓練模型了。

$ rosrun sensor_stick train_svm.py

運行此命令后，將在終端上獲得一些有關分類器總體準確性的文本輸出，并且將彈出兩個圖，顯示分類器對各種對象的相對準確性：

這些圖顯示了分類器的兩個不同版本的混淆矩陣。左邊是原始計數，右邊是占總數的百分比。假設在特征生成過程中以隨機方向生成對象，所以每次生成的圖都不一樣。

運行上面的命令還將導致訓練的模型保存在model.sav文件中。

注意：此model.sav文件將保存在catkin_ws文件夾中。

改善模型

我們的混淆矩陣生成的非常不理想。是因為還沒有真正生成有意義的特性。要獲得更好的特性，在/sensor_stick/src/sensor_stick/中打開features.py腳本(這可能看起來像一個奇怪的目錄結構，但這是設置內部Python包的首選ROS方法)。在這個腳本中，有兩個名為compute_color_histograms()和compute_normal_histograms()的函數。

在compute_color_histograms()和compute_normal_histograms()函數中，有從點云中提取的三個值列表，其中channel_*_vals(表示顏色)和norm_*_vals(表示法線)。可以使用之前提到的直方圖技術來存儲這些數據。在加入直方圖之后，將它們連接到一個特征向量中并進行標準化，以創建函數輸出(normed_features)。再次運行capture_features.py，train_svm.py查看效果。

features.py函數如下所示：

import matplotlib.colors
import matplotlib.pyplot as plt
import numpy as np
from pcl_helper import *def rgb_to_hsv(rgb_list):rgb_normalized = [1.0*rgb_list[0]/255, 1.0*rgb_list[1]/255, 1.0*rgb_list[2]/255]hsv_normalized = matplotlib.colors.rgb_to_hsv([[rgb_normalized]])[0][0]return hsv_normalizeddef compute_color_histograms(cloud, using_hsv=False):# Compute histograms for the clusterspoint_colors_list = []# Step through each point in the point cloudfor point in pc2.read_points(cloud, skip_nans=True):rgb_list = float_to_rgb(point[3])if using_hsv:point_colors_list.append(rgb_to_hsv(rgb_list) * 255)else:point_colors_list.append(rgb_list)# Populate lists with color valueschannel_1_vals = []channel_2_vals = []channel_3_vals = []for color in point_colors_list:channel_1_vals.append(color[0])channel_2_vals.append(color[1])channel_3_vals.append(color[2])# TODO: Compute histogramschannel_1_hist = np.histogram(channel_1_vals, bins=32, range=(0, 256))channel_2_hist = np.histogram(channel_2_vals, bins=32, range=(0, 256))channel_3_hist = np.histogram(channel_3_vals, bins=32, range=(0, 256))hist_features = np.concatenate((channel_1_hist[0], channel_2_hist[0], channel_3_hist[0])).astype(np.float64)# TODO: Concatenate and normalize the histogramsnormed_features = hist_features / np.sum(hist_features)return normed_features def compute_normal_histograms(normal_cloud):norm_x_vals = []norm_y_vals = []norm_z_vals = []for norm_component in pc2.read_points(normal_cloud,field_names = ('normal_x', 'normal_y', 'normal_z'),skip_nans=True):norm_x_vals.append(norm_component[0])norm_y_vals.append(norm_component[1])norm_z_vals.append(norm_component[2])# TODO: Compute histograms of normal values (just like with color)channel_1_hist = np.histogram(norm_x_vals, bins=32, range=(0, 256))channel_2_hist = np.histogram(norm_y_vals, bins=32, range=(0, 256))channel_3_hist = np.histogram(norm_z_vals, bins=32, range=(0, 256))hist_features = np.concatenate((channel_1_hist[0], channel_2_hist[0], channel_3_hist[0])).astype(np.float64)# TODO: Concatenate and normalize the histogramsnormed_features = hist_features / np.sum(hist_features)return normed_features

create_features.py如下所示

#!/usr/bin/env python
import numpy as np
import pickle
import rospyfrom sensor_stick.pcl_helper import *
from sensor_stick.training_helper import spawn_model
from sensor_stick.training_helper import delete_model
from sensor_stick.training_helper import initial_setup
from sensor_stick.training_helper import capture_sample
from sensor_stick.features import compute_color_histograms
from sensor_stick.features import compute_normal_histograms
from sensor_stick.srv import GetNormals
from geometry_msgs.msg import Pose
from sensor_msgs.msg import PointCloud2def get_normals(cloud):get_normals_prox = rospy.ServiceProxy('/feature_extractor/get_normals', GetNormals)return get_normals_prox(cloud).clusterif __name__ == '__main__':rospy.init_node('capture_node')models = [\'beer','bowl','create','disk_part','hammer','plastic_cup','soda_can']# Disable gravity and delete the ground planeinitial_setup()labeled_features = []for model_name in models:spawn_model(model_name)for i in range(10):# make five attempts to get a valid a point cloud then give upsample_was_good = Falsetry_count = 0while not sample_was_good and try_count < 5:sample_cloud = capture_sample()sample_cloud_arr = ros_to_pcl(sample_cloud).to_array()# Check for invalid clouds.if sample_cloud_arr.shape[0] == 0:print('Invalid cloud detected')try_count += 1else:sample_was_good = True# Extract histogram featureschists = compute_color_histograms(sample_cloud, using_hsv=True)normals = get_normals(sample_cloud)nhists = compute_normal_histograms(normals)feature = np.concatenate((chists, nhists))labeled_features.append([feature, model_name])delete_model()pickle.dump(labeled_features, open('training_set.sav', 'wb'))

再次運行train_svm.py，得到的輸出如下，可以看到，得到的混淆矩陣和結果好了很多。

要修改每個對象隨機派生的次數，在capture_features.py中查找以range(5)中的for i in range(5)的for循環。增加此值以增加為每個對象捕獲特性的次數。

使用HSV，在capture_features.py中找到調用compute_color_histograms()的行，并將標志更改為using_hsv=True。

SVM訓練過程，在train_svm.py中：

#!/usr/bin/env python
import pickle
import itertools
import numpy as np
import matplotlib.pyplot as plt
from sklearn import svm
from sklearn.preprocessing import LabelEncoder, StandardScaler
from sklearn import cross_validation
from sklearn import metricsdef plot_confusion_matrix(cm, classes,normalize=False,title='Confusion matrix',cmap=plt.cm.Blues):"""This function prints and plots the confusion matrix.Normalization can be applied by setting `normalize=True`."""if normalize:cm = cm.astype('float') / cm.sum(axis=1)[:, np.newaxis]plt.imshow(cm, interpolation='nearest', cmap=cmap)plt.title(title)plt.colorbar()tick_marks = np.arange(len(classes))plt.xticks(tick_marks, classes, rotation=45)plt.yticks(tick_marks, classes)thresh = cm.max() / 2.for i, j in itertools.product(range(cm.shape[0]), range(cm.shape[1])):plt.text(j, i, '{0:.2f}'.format(cm[i, j]),horizontalalignment="center",color="white" if cm[i, j] > thresh else "black")plt.tight_layout()plt.ylabel('True label')plt.xlabel('Predicted label')# 從磁盤加載培訓數據
training_set = pickle.load(open('training_set.sav', 'rb'))# 將特性和標簽格式化，以便與scikit learn一起使用
feature_list = []
label_list = []for item in training_set:if np.isnan(item[0]).sum() < 1:feature_list.append(item[0])label_list.append(item[1])print('Features in Training Set: {}'.format(len(training_set)))
print('Invalid Features in Training set: {}'.format(len(training_set)-len(feature_list)))X = np.array(feature_list)
# Fit a per-column scaler
X_scaler = StandardScaler().fit(X)
# Apply the scaler to X
X_train = X_scaler.transform(X)
y_train = np.array(label_list)# 將標簽字符串轉換為數字編碼
encoder = LabelEncoder()
y_train = encoder.fit_transform(y_train)# 創建分類器
clf = svm.SVC(kernel='linear')# 建立5倍交叉驗證
kf = cross_validation.KFold(len(X_train),n_folds=5,shuffle=True,random_state=1)# 進行交叉驗證
scores = cross_validation.cross_val_score(cv=kf,estimator=clf,X=X_train,y=y_train,scoring='accuracy')
print('Scores: ' + str(scores))
print('Accuracy: %0.2f (+/- %0.2f)' % (scores.mean(), 2*scores.std()))# 收集預測
predictions = cross_validation.cross_val_predict(cv=kf,estimator=clf,X=X_train,y=y_train)accuracy_score = metrics.accuracy_score(y_train, predictions)
print('accuracy score: '+str(accuracy_score))confusion_matrix = metrics.confusion_matrix(y_train, predictions)class_names = encoder.classes_.tolist()# 訓練分類器
clf.fit(X=X_train, y=y_train)model = {'classifier': clf, 'classes': encoder.classes_, 'scaler': X_scaler}# 將分類器保存到磁盤
pickle.dump(model, open('model.sav', 'wb'))# 繪制非標準化混淆矩陣
plt.figure()
plot_confusion_matrix(confusion_matrix, classes=encoder.classes_,title='Confusion matrix, without normalization')# 繪制歸一化混淆矩陣
plt.figure()
plot_confusion_matrix(confusion_matrix, classes=encoder.classes_, normalize=True,title='Normalized confusion matrix')plt.show()

來看一下model.sav文件中的信息：

{
'classes'	:	array(['beer', 'bowl', 'create', 'disk_part', 'hammer', 'plastic_cup','soda_can'], dtype='|S11'), 
'classifier':	SVC(C=1.0, cache_size=200, class_weight=None, coef0=0.0,decision_function_shape='ovr', degree=3, gamma='auto', kernel='linear',max_iter=-1, probability=False, random_state=None, shrinking=True,tol=0.001, verbose=False), 
'scaler'	:	StandardScaler(copy=True, with_mean=True, with_std=True)
}

物體識別

首先，必須構建節點來分割點云。

復制sensor_stick/scripts/目錄中的template.py文件，并將其命名為類似object_recognition.py的名稱。

首先，創建一些要接收的空列表

    # Classify the clusters!detected_objects_labels = []detected_objects = []

接下來，編寫一個for循環來遍歷每個分段的集群。

	# 遍歷各個集群，以索引和點的列表for index, pts_list in enumerate(cluster_indices):# 使用之前練習的程序pcl_cluster = cloud_objects.extract(pts_list)# TODO: convert the cluster from pcl to ROS using helper functioncloud_cluster = pcl_to_ros(pcl_cluster)# 提取直方圖特征# TODO: complete this step just as is covered in capture_features.py# 獲取色彩（color）直方圖chists = compute_color_histograms(cloud_cluster, using_hsv=True)# 計算法線（normal）的直方圖normals = get_normals(cloud_cluster)nhists = compute_normal_histograms(normals)# 將色彩和法線直方圖聯結作為特征feature = np.concatenate((chists, nhists))# 預測prediction = clf.predict(scaler.transform(feature.reshape(1,-1)))# 標簽從數字轉換為字符label = encoder.inverse_transform(prediction)[0]detected_objects_labels.append(label)# 定義標簽位置，將標簽發布到RVizlabel_pos = list(white_cloud[pts_list[0]])label_pos[2] += .4object_markers_pub.publish(make_label(label,label_pos, index))# 將檢測到的對象添加到檢測到的對象列表中。do = DetectedObject()do.label = labeldo.cloud = cloud_clusterdetected_objects.append(do)rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))# Publish the list of detected objectsdetected_objects_pub.publish(detected_objects)

下面，在if……name__ == '……main__'開頭的部分，添加以下代碼來創建一些新的發布者，并加載訓練完成的模型中:

	# create two publishersobject_markers_pub = rospy.Publisher('/object_markers', Marker, queue_size=1)detected_objects_pub = rospy.Publisher('detecter_objects', DetectedObjectsArray, queue_size=1)# 加載模型model = pickle.load(open('model.sav', 'rb'))clf = model['classifier']# 用0和n_classes-1之間的值對標簽進行編碼。encoder = LabelEncoder()encoder.classes_ = model['classes']# 定標器scaler = model['scaler']

完整程序如下,在ros中不要使用中文注釋：

#!/usr/bin/env pythonimport numpy as np
import sklearn
from sklearn.preprocessing import LabelEncoderimport picklefrom sensor_stick.srv import GetNormals
from sensor_stick.features import compute_color_histograms
from sensor_stick.features import compute_normal_histograms
from visualization_msgs.msg import Markerfrom sensor_stick.marker_tools import *
from sensor_stick.msg import DetectedObjectsArray
from sensor_stick.msg import DetectedObject
from sensor_stick.pcl_helper import *# 定義獲取點云法線的函數
def get_normals(cloud):get_normals_prox = rospy.ServiceProxy('/feature_extractor/get_normals', GetNormals)return get_normals_prox(cloud).cluster# Callback function for your Point Cloud Subscriber
def pcl_callback(pcl_msg):# 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()# 分類集群(loop through each detected cluster one at a time)# 初始化目標數組和標簽數組detected_objects_labels = []detected_objects = []# 遍歷各個集群，以索引和點的列表for index, pts_list in enumerate(cluster_indices):# 使用之前練習的程序pcl_cluster = cloud_objects.extract(pts_list)# TODO: convert the cluster from pcl to ROS using helper functioncloud_cluster = pcl_to_ros(pcl_cluster)# 提取直方圖特征# TODO: complete this step just as is covered in capture_features.py# 獲取色彩（color）直方圖chists = compute_color_histograms(cloud_cluster, using_hsv=True)# 計算法線（normal）的直方圖normals = get_normals(cloud_cluster)nhists = compute_normal_histograms(normals)# 將色彩和法線直方圖聯結作為特征feature = np.concatenate((chists, nhists))# 預測prediction = clf.predict(scaler.transform(feature.reshape(1,-1)))# 標簽從數字轉換為字符label = encoder.inverse_transform(prediction)[0]detected_objects_labels.append(label)# 定義標簽位置，將標簽發布到RVizlabel_pos = list(white_cloud[pts_list[0]])label_pos[2] += .4object_markers_pub.publish(make_label(label,label_pos, index))# 將檢測到的對象添加到檢測到的對象列表中。do = DetectedObject()do.label = labeldo.cloud = cloud_clusterdetected_objects.append(do)rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))# Publish the list of detected objectsdetected_objects_pub.publish(detected_objects)if __name__ == '__main__':# ROS node initializationrospy.init_node('clustering', anonymous=True)# Create Subscriberspcl_pub = rospy.Subscriber('/sensor_stick/point_cloud', pc2.PointCloud2, pcl_callback, queue_size=1)# create two publishersobject_markers_pub = rospy.Publisher('/object_markers', Marker, queue_size=1)detected_objects_pub = rospy.Publisher('detecter_objects', DetectedObjectsArray, queue_size=1)# 加載模型model = pickle.load(open('model.sav', 'rb'))clf = model['classifier']# 用0和n_classes-1之間的值對標簽進行編碼。encoder = LabelEncoder()encoder.classes_ = model['classes']# 定標器scaler = model['scaler']# 初始化color_listget_color_list.color_list = []# TODO: Spin while node is not shutdownwhile not rospy.is_shutdown():rospy.spin()

然后新開啟一個終端,按原來的操作，進行如下命令

$ roslaunch sensor_stick robot_spawn.launch

在另一個終端中，運行對象識別節點（model.sav文件必須與運行此文件的目錄位于同一目錄中）：

$ chmod +x object_recognition.py
$ ./object_recognition.py

輸出結果如下：

還是有兩個模型不知道加載到哪里去了，但是剩下的幾個模型還是很成功的判斷出來了。

總結

以上是生活随笔為你收集整理的Udacity机器人软件工程师课程笔记（二十二) - 物体识别 - 色彩直方图，支持向量机SVM的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。