最近在udacity上學習了基於ROS的點雲識別模型。所以想寫這個文章來回顧一下思路和做一下總結。

這個項目的主要感測器是RGB-D攝像機，也就是深度攝像機。（之前的文章介紹過安裝深度攝像機的。那部分的內容是為了將這個項目代碼用於實踐。）利用深度攝像機傳輸的數據對特定物體進行識別，最終再由機器臂將識別的物體放入之前定義好的箱子裏。也就是說，這裡面最主要的任務就是令我攝像機傳輸的數據，對物體進行識別。因為是用的是深度攝像機，所以從攝像機裏傳輸出來的數據是RGB和Point cloud數據。所以感知模塊利用這些傳輸過來的數據對物體進行聚類和識別。識別出來物體的種類之後，我們可以根據點雲信息確定物體的實際位置。在控制機器臂。去家取那個位置的物體。但是呢，在這個項目當中，因為時間的問題，利用機器臂夾取物體的代碼，雖然參考別人的代碼寫的上去，但是這部分的代碼實際上並沒有作用在模擬環境裡面，所以這邊我只介紹感知相關的內容。

---

首先，要完成這個項目，大體內容分成以下幾個部分。

1. 首先是要構建基於ROS的模擬環境

2. 添加關於感知模塊的所有代碼

https://github.com/Fred159/3D-Perception/blob/master/pr2_robot/scripts/project_template.py?

github.com

3. 最終在基於ROS的模擬環境裏，通過運行python文件進行物體識別及輸出結果

Fred159/3D-Perception?

github.com

---

1 搭建ROS模擬環境

機器人操作系統的模擬環境搭建起來其實不是很麻煩。在這裡我們直接利用了優達學城給出的VM image. 這個鏡像裡麪包含了。這個項目所需要的所有的庫。VM是什麼呢？翻譯過來叫虛擬機。這個到底是做什麼的呢？為什麼非要用這個東西？

再決定要不要用這個東西之前我們先要了解一下這個東西是什麼。要知道我們大部分運行環境都是基於Windows系統的。但是ROS是要運行在linux系統上的。不過我們又不想把我們的電腦的系統完全換成linux。所以我們就需要我們現在的電腦可以虛構出一個環境，給這個環境一定的許可權和存儲空間，讓linux系統在虛擬環境裡面運行。所以VM叫做虛擬機。顧名思義就是虛擬的機器的意思。把所有的東西放到這個windows 操作系統下的虛擬機裏運行，就相當於運行在linux系統下運行了。這就是為什麼我們用虛擬機的原因。如果說你的操作系統本身就是linux，那麼你就直接可以在這個環境裡面搭建ROS。（可以找找資源，可以直接用一行代碼就可以安裝好ROS平臺）當然，相關的庫，還是需要安裝的。

在這裡我直接使用了優達學城給出的鏡像。這個鏡像也可以選擇不用，所有需要的庫自己安裝。我只是想圖個方便，所以直接用了這個鏡像。鏡像的運行需要vmplayer，鏈接如下。但是，需要說明的一點就是這個鏡像裡面的有些內容，比如sklearn的版本是需要修改的。

https://www.vmware.com/products/workstation-player.html?

www.vmware.com

在這裡我想提醒一下聽這個課程的同學。udacity提供的命令行裡面，ros的launch 文件們總是會報錯。但是隻要你們的Gazebo 和Rviz可以正常打開，那麼就可以運行相關的識別文件。別看它一直報錯，但是這跟你運行識別相關的代碼是沒有太多的相關性的。因為有些內容是涉及機器臂的運行的。不涉及操作機器臂的時候，可以直接無視一些報錯。當然報錯的東西越少越好。

2 添加關於感知模塊的所有代碼

在開始講述代碼之前，再說明一下這次項目的內容。據他們的udacity官方的介紹說這個項目的起源是亞馬遜的機器人挑戰賽。挑戰賽的內容就是。官方會給出一組數據分揀清單，然後由參賽人員把分揀清單輸入給機器人系統中。輸入命令後，由機器人通過自身的所有系統去判斷前方的物體種類，位置，大小，然後最終判斷物體應該放到左邊的箱子還是右邊的箱子，最後由機器人操控機械臂，把特定的物體放到左邊或右邊的箱子裡面。

這裡就需要明確input 和output

• 感知模塊input： pick list
• 感知模塊output：物體label和這個label代表的點雲集和RGB集（實際上由RGB-D攝像機一個感測器輸出，所以接受的是一組數據XYZRGB）
• 機器臂指令輸出代碼input = 感知模塊的output
• 機器臂指令輸出代碼output = TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE

這裡需要重點說明一下的就是，我們現在所說的機器臂指令輸出代碼，指的是發布命令的代碼。那就會有人問更底層的那些基本操作代碼在哪裡？也在這個repo裏。 但是，這部分是udacity 提供的代碼。並不是我個人編寫的。 以後有時間或者必要的時候，我也會去分析底層的代碼。 涉及的內容有forward， inverse kinematic 和trajectory generation。 不過，對於無人車來說，這些東西好像不是很必要。。。。

--我編寫的部分就是感知代碼，其他基本都是udacity 提供的--

那麼就正式開始代碼講解。

repo傳送陣~~

https://github.com/Fred159/3D-Perception/blob/master/pr2_robot/scripts/project_template.py?

github.com

對於人類這種視覺動物，我們首先要看到我們需要解決的問題，再去考慮要去解決什麼樣的問題。所以我們需要看到點雲RGB數據。如下圖

點雲&RGB raw data（偽）（這個當然是被udacity提前處理過。原始數據，肯定不會這麼整齊）

將上面這些點雲數據處理之後，可視化的結果如下圖所示。

Rviz畫面

識別出來的label。每個label也對應著一堆XYZRGBdata

那麼為了識別這些物品，我們需要整理一下，處理這些數據的步驟。

2.1 Noise filtering （statistical filter）

2.2 Voxel grid downsampling

2.3 Passthrough filter

2.4 RANSAC plane segmentation

2.5 From segmentation extract outliers and inliers

2.6 Euclidean clustering

2.7 Generate training data

2.8 Train SVM model to learn specific features of each model

2.9 Process the clusters by using color histogram and norms histogram

2.10 Combine label and cluster with DetectedObject class and publish to ROS

2.11 ROS receives detected_object and show labels in Rviz

(到這裡,感知模塊就結束了,最終輸出了label和cluster的數據,之後就是機器臂接收這些數據並執行夾取,移動,放置操作)

（2.12）（機器臂） 讀取左右兩個箱子的位置, 根據detected_list 找到要找的物體,並根據那個label的cluster和forward kinematic找到夾取位置, 放爪子去抓,然後再根據path generation的trajectory執行inverse kinematic ,最終拿起,放到左或者右的箱子裏. 重複. 嗯....這邊就比較複雜了.而且這些都是udacity 提供的.感興趣的話,大家可以去看看.講解代碼並不涉及這部分的內容。

---

2.1 Noise filtering （statistical filter）

這個濾波器的作用是用來過濾雜訊的。為什麼要過濾雜訊呢？因為現實世界中所有的感測器都是有雜訊的，感測器不可能只獲取我們想要的那些信息。感測器輸出的內容只是提取出來符合特徵的電壓值而已。對於我們來說，識別一個物體，需要從感測器每秒鐘獲得成千上萬甚至是幾百萬的數據。但是我們並不需要對他們每一個數據都進行仔細的處理。我們只需要提取最重要的特徵就可以了。而這種最重要的特徵，並不是一兩個雜訊點之間的關係，而是一堆一堆數據之間的關係。而且雜訊還會影響特徵提取。

How？

我們通過，設定一個值k和x（就是正態分佈），然後。通過對這個值周邊一定數量的值，進行取平均。那麼，我們不僅可以提取一部分數據的特徵，還可以去除最大值最小值對特徵值的影響。具體代碼如下。

print(statistical outlier filtering)
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(10)
x = 0.01
outlier_filter.set_std_dev_mul_thresh(x)
cloud_after_statistical_filter = outlier_filter.filter()


2.2 Voxel grid downsampling

voxel 這個東西是啥呢？ 翻譯過來叫體素。對應的名字是圖像裡面的像素。像素就是2d圖像裡面的最小的單元。那麼體素就是3d點雲的最小單元。『我的世界』這個遊戲不知道大家知不知道。 如果這個遊戲是一個點雲集，那麼這個遊戲裡面的每個小格子就是每一個體素

我的世界官宣圖片

為什麼要用體素去分割點雲呢？因為這樣做可以提取更多有效的特徵信息。據他們介紹是這樣的，但是如果要讓我說用式子去推導，我也不知道。。。

具體代碼實現如下。leaf_size就是定義最小的體素的長寬高。這裡設定的是1cm。

TODO: Voxel Grid Downsampling
vox = cloud_after_statistical_filter.make_voxel_grid_filter()
LEAF_SIZE = 0.01
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_after_downsampling = vox.filter()
filename = cloud_after_downsampling.pcd
pcl.save(cloud_after_downsampling, filename)


2.3 Passthrough filter

這是劃分區域的filter。 跟圖像處理時所用的ROI（region of interest）是一樣的概念。我們不需要除了想要識別的物體以外的視野和數據。我們只想要識別的物體上的點雲。所以我們就利用這個fitler 把桌子和其他亂七八糟的點雲一併過濾掉，然後把想要的ROI裡面的東西提取出來。

設定數據ROI

# TODO: PassThrough Filter
print(start passthrough fitler run)
# y axis
cloud = cloud_after_downsampling.make_passthrough_filter()
filter_axis = x
cloud.set_filter_field_name(filter_axis)
axis_min = 0.35
axis_max = 0.8
cloud.set_filter_limits(axis_min, axis_max)
cloud_after_x_axis_limit = cloud.filter()
print(start passthrough filter in x axis)
# z axis
cloud = cloud_after_x_axis_limit.make_passthrough_filter()
filter_axis = z
cloud.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 0.9
cloud.set_filter_limits(axis_min, axis_max)
cloud_after_x_z_limit = cloud.filter()
filename = cloud_after_x_z_limit.pcd
pcl.save(cloud_after_x_z_limit, filename)


2.4 RANSAC plane segmentation

這裡我們想要把要識別的物體跟桌子去分開來。這裡用的是RANSAC匹配演算法。 是點雲匹配e 經典演算法。

百度百科:

ransac_百度百科?

baike.baidu.com

（這個庫是本田汽車贊助維持的，佈局真早）PCL（point cloud Lib） 庫：

Point Cloud Library (PCL)?

pointclouds.org

這裡通過設定距離的方式區分inlier和outlier。 這裡的距離設定為0.01。也就是1cm

具體代碼如下

print(start ransac plane segmentation)
# TODO: RANSAC Plane Segmentation
seg = cloud_after_x_z_limit.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)

max_dis = 0.01
seg.set_distance_threshold(max_dis)

2.5 From segmentation extract outliers and inliers

從分割好的模型中提取inlier 和outlier點。

代碼實現：

print(Extract inliers and outliers)
# TODO: Extract inliers and outliers
inliers, coefficients = seg.segment()

extracted_inliers = cloud_after_x_z_limit.extract(inliers, negative=False)
extracted_outliers = cloud_after_x_z_limit.extract(inliers, negative=True)

filename = extracted_inliers.pcd
pcl.save(extracted_inliers, filename)

filename = extracted_outliers.pcd
pcl.save(extracted_outliers, filename)

最終，所有的點符合最大距離是0.01m的點集基本就是桌子這個平面了。而其他不符合這個最大距離0.01m限制的點，就是我們要處理的物體的點雲集了。這裡叫segmentation。叫分割。顧名思義，就是分割背景和特定物體。那麼，注意，這裡的輸出結果還是一堆點雲，我們的演算法並不知道這些點雲之間有啥關係。雖然我們一眼就能區分開各個點雲屬於不同的類，但是計算機還不知道。所以之後的步驟就是要把這些點雲分割成一個個的類。

2.6 Euclidean clustering

Euclidean clustering直接翻譯過來就是根據距離進行分類。 也就是說，數據裡面的點和這個點周邊的點的距離小於一定的閾值的話，這些個點就同屬一個類。

當然這種演算法很多，這裡根據課程提示，直接選用了kd樹的方式去聚類。kd樹演算法，簡單點就是通過二叉樹，將點雲基於很多個超平面分隔開。在百度上看了這個博客。應該是位大牛寫的。 大概瞭解了一下，也沒深看。因為現在是調用庫，並沒有深入瞭解的必要。

KNN（三）--KD樹詳解及KD樹最近鄰演算法 - 工作筆記 - CSDN博客?

blog.csdn.net

print(Eucliean clustering)
# TODO: Euclidean Clustering
# ros_cloud_objects = pcl_to_ros(extracted_outliers)
# pcl_objects_pub.publish(ros_cloud_objects)
white_cloud = XYZRGB_to_XYZ(extracted_outliers)
tree = white_cloud.make_kdtree()
# 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.03)
ec.set_MinClusterSize(11)
ec.set_MaxClusterSize(45000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
print(create cluster-mask point cloud)


上面的代碼就是在python上實現kd tree的代碼。可以看到通過ec.xxxx的方式設置了kdtree的相關閾值。

下面這個部分的代碼就是給與這些區分好的點雲顏色，並通過ros public到rviz中，進行可視化的操作。不同的顏色就是不同的類。

# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# Assign a color corresponding to each segmented object in scene
color_cluster_point_list = []
cluster_color = get_color_list(len(cluster_indices))

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)
# TODO: Convert PCL data to ROS messages
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
print(publish pcl_objects_pub messages)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cluster_cloud)

2.7 Generate training data

嗯？ 忽然蹦出來要生成訓練集？

是的。 因為在2.6中，我們只是把點雲數據進行了聚類，但是這些個類，或者說這個形狀點雲到底是個什麼東西，我們就不得而知了。

所以我們需要構建一個識別演算法去識別點雲數據的特徵。這裡採用了SVM去識別（我感覺其實也就是一種有label的聚類。。。）。當然，其他學院也有用了深度學習的框架。我還沒看那個人的代碼，之後去看看

這個項目裏，運用的感測器因為是RGB-D攝像機，所以得到的數據有點雲和點雲的RGB。直到聚類，用的數據是點雲。而識別，就不是靠電雲了，靠的是RGB。

靠RGB的話，我們就需要生成各個角度的RGB圖像，然後把這些訓練圖像feed給SVM。讓SVM學習這些數據的特徵。

所以，我們需要生成數據來訓練svm。訓練圖像是通過uda提供的python文件，控制rviz裡面的物體模型來獲得的。代碼如下。

https://github.com/Fred159/3D-Perception/blob/master/sensor_stick/scripts/capture_features.py?

github.com

2.8 Train SVM model to learn specific features of each model

（2.9 Process the clusters by using color histogram and norms histogram）

訓練SVM也是通過uda提供的python來進行訓練的。之所以把2.9也放進這裡面，主要是因為，feature capture 的這個python文件提取的特徵就是color 直方圖和norm 直方圖。而訓練的時候，SVM也是通過提取相應圖片的color 直方圖和norm 直方圖對物體進行識別和分類的。

https://github.com/Fred159/3D-Perception/blob/master/sensor_stick/scripts/train_svm.py?

github.com

訓練出來的svm給出的confusion matrix 如下。 我們可以看到訓練好的模型可以較好的預測相應的物體。下圖中，左邊的是沒有normalize的，而右邊是normalize過後的。

2.10 Combine label and cluster with Detected Object class and publish to ROS

訓練好了SVM，那麼也就是說，機器人已經有了識別的能力。當我們，打開launch文件的時候，機器人開始識別並把數據用[[label1, [point cloud set1]],label2,[point cloud set2],[xxxxxx],[xxxxxxx]]的方式傳輸給ROS master。

那麼如果想通過機器臂夾取某個東西的話，此時我們通過ros master給機器人一個指令，讓機器人夾取某個label，比如label2。那麼機器人就會通過label2的point cloud set2 來判斷物體的位置和機器臂可以夾取的點（當然這些東西，每一個都是一個領域。。。這裡就簡單的用模擬意思意思，說明瞭一下。。）。

這次還沒有做到夾取物體的程度。這裡就不再過多闡述。

2.11 ROS receives detected_object and show labels in Rviz

最後，為了方便看結果，通過RVIZ可視化了label。這些個label和label在rviz裡面的位置也都是通過最後處理好的[[label1, [point cloud set1]],label2,[point cloud set2],[xxxxxx],[xxxxxxx]]來進行可視化的。就是label1放在point cloud set1的平均值以上多少距離的位置。

其實，只要有[[label1, [point cloud set1]],label2,[point cloud set2],[xxxxxx],[xxxxxxx]]這部分的數據，之後的所有東西就都簡單了。

3. 最終在基於ROS的模擬環境裏，通過運行python文件進行物體識別及輸出結果

檢測結果顯示，3個，5個的效果都還可以。但是在8個物體的時候，有些物體是被擋住了一部分，所以檢測結果並不是很理想。這部分的檢測能力還需要提高。

---

4 總結

本次文章通過uda的項目，對基於點雲和RGB數據的識別問題進行了簡單的介紹。

當然，識別遠遠不止這些個內容，但是我認為這個項目可以很好的讓我們對基於點雲和RGB識別演算法的理解更深一步。同時因為運行環境是通過ROS進行的，所以也簡單的瞭解了ROS的系統架構。涉及到的知識點有，pcl庫，點雲的過濾，ROI過濾，聚類，分類，RGB圖像的特徵提取， 識別，ROS等等。

希望本篇文章可以讓沒有經驗的同學少走一些彎路。

如有幫助，別忘了關注+點個贊哦~

20181204

研究室

推薦閱讀：

相關文章