任務主線是深度相機看到物體是什麼、在哪兒，位姿傳送給機械臂進行抓取，這兩週主要解決了用深度相機檢測一個可樂罐以及釋出其位置資訊。


執行環境：Ubuntu16.04+ROS Kinetic+PCL

目前深度相機主要的方法有：

1.基於霍夫變換

（可以參考2010年的論文 ppf (point pair feature)：Model Globally, Match Locally: Efficient and Robust 3D Object Recognition）

2.基於模板匹配（也就是本文采用的基於linemod演算法）比1效果要更好

論文：Multimodal Templates for Real-Time Detection of Texture-less Objects in Heavily Cluttered Scenes

http://campar.in.tum.de/pub/hinterstoisser2011linemod/hinterstoisser2011linemod.pdf

3.基於patch匹配+random forest（Latent-Class Hough Forests 用於處理linemd在遮擋時候識別率下降的問題）

論文：Learning 6D Object Pose Estimation using 3D Object Coordinates

LCHF：https://arxiv.org/abs/1706.03285

4.基於點雲（也是之前一週採用的方法）

http://wiki.ros.org/pcl_ros/Tutorials

http://ros-developer.com/2017/05/15/object-recognition-and-6dof-pose-estimation-with-pcl-pointcloud-and-ros/
（核心專案參考 最下面有個評論以及回覆可以作為參考後面就捨棄掉這種方法了）
也就是
https://github.com/adityag6994/3D_Object_Seg_CNN
以及可以參考他的分享
https://github.com/adityag6994/object_tracking_particle_filter

https://blog.csdn.net/AmbitiousRuralDog/article/details/80268920
（地面點雲分割）

5.基於CNN end-to-end

論文：SSD-6D: Making RGB-based 3D detection and 6D pose estimation great agai

關於linemod演算法可以參考：

（http://campar.in.tum.de/pub/hinterstoisser2011linemod/hinterstoisser2011linemod.pdf以及https://blog.csdn.net/zmdsjtu/article/details/79933822）這裡稍作簡單介紹已使文章內容更加完整嚴謹

linemod主要解決的是3D剛性物體的實時檢測與定位，利用較短的訓練時間訓練採集的RGBD資訊，將其與模版匹配對比後得到物體資訊、位姿及自信度。
linemod特徵採用彩色影象的梯度資訊結合物體表面的法向特徵作為模板匹配的依據：

1.首先計算梯度方向，分別進行77高斯模糊、sobel運算元計算梯度、每個畫素點梯度方向及幅值求解（去掉低於閾值的梯度）、33鄰閾內統計梯度方向，最終對梯度起到主成分提取和放大的提取特徵效果。

2.接著方向擴散使匹配具備容錯度，影象提取得到特徵圖後在一定鄰閾讓特徵進行擴散、利用模版進行滑窗匹配得到容錯度。

3.預處理響應圖，對八個方向及處理得到的擴散圖逐個畫素匹配，得到距離最近方向角度的餘弦值，針對每個方向遍歷圖片計算匹配結果。

4.線性儲存3預處理的結果，並擴充套件到深度圖。



好了開始這兩週的工作內容總結：

執行環境：Ubuntu16.04+ROS Kinetic+PCL+ORK

核心是參考這兩篇文章，但是其是針對Kinetic或者Xbox，所以並不完全適用

http://wg-perception.github.io/ork_tutorials/tutorial03/tutorial.html#setup-the-working-environment(ORK官網 這裡有完整詳細的步驟)

https://blog.techbridge.cc/2016/05/14/ros-object-recognition-kitchen/(核心是一樣的，只不過是個人的應用)

接下來進行步驟總結：

**1. 下載realsense適用Intel ZR300深度相機的例程包（http://wiki.ros.org/realsense_camera）
官網下載並安裝，選擇branch(indigo-devel)。作為替代openni2(適用Kinetic)的啟動程式碼，可以得到實時影象的RGBD影象並在rviz顯示，這裡的-b （分支名）要新增，不然下載下來的是realsense2_camera 新版本的在kinetic編譯不通。(注意ZR300要接USB3.0介面！)

#git clone -b indigo-devel https://github.com/intel-ros/realsense.git
#roslaunch realsense_camera zr300_nodelet_rgbd.launch
#rviz

注意ros下執行的包要裝到~/catkin_ws/src目錄下編譯執行！

話題名為：depth_registered

其釋出了RGBD資訊為：(這樣就是有釋出者了，下面的資訊以後會用到)

Rgb_frame_id:’camera_rgb_frame’

Rgb_image_topic:’/camera/rgb/image_rect_color’

Rgb_camera_info:’/camera/rgb/camera_info’

Depth_frame_id:’camera_depth_info’

Depth_image_topic:’/camera/depth_registered/sw_registered/image_rect’

Depth_camera_info:’/camera/depth_registered/sw_registered/camera_info’

注意類似這樣的錯誤不能正常啟動的話可以重新插拔realsense攝像頭，或者按下面所說的在rqt_reconfigure重新打鉤enable就可以了。

#rosrun rqt_reconfigure rqt_reconfigure

陸陸續續有很多Bug，如果提示缺少什麼什麼包的話直接下載後重新編譯即可，這裡列出最近的一個：roslaunch realsense_camera zr300_nodelet_rgbd的時候出現:

IOError: [Errno 13] Permission denied .ros/rosdep/sources.cache/index

解決：

#sudo rosdep update
#apt-get update
#rospack profile

更新一下就好，並且在整個工程試驗中要經常#sudo apt-get update 以及 #rospack profile，前者更新包以避免不必要的麻煩，後者是ros有時候會找不到檔案的位置導致無法tap。
參考：https://answers.ros.org/question/281477/ioerror-errno-13-permission-denied-rosrosdepsourcescacheindex/
2. 安裝ork需要用到的包
注：所有的資源都可以用shadowsocks翻牆來下,超級好用,輸入命令時只需要將proxychains加在所有命令前即可，如果有sudo就加在其後面，這樣下載速度真的超級快.

Eg.#sudo proxychains apt-get install ros-kinetic-object-recognition-core

博主在安裝過程中出錯後重新選擇了從source安裝：

#git clone http://github.com/wg-perception/object_recognition_msg
#git clone http://github.com/wg-perception/object_recognition_ros-bisualization
#cd ../ && catkin_make

（可以用proxychains git clone…如果裝了shadowsocks的話）

3. 安裝linemod

#git clone http://github.com/wg-perception/object_recognition_core
#git clone http://github.com/wg-perception/linemod
#git clone http://github.com/wg-perception/ork_renderer
#cd ../ && catkin_make

如果之前有配置過Ros，在src目錄直接#cm即可。在這個過程彙總可以挨個下載後即編譯，解決了出現的問題後再下載其他的繼續編譯，以此發現其中哪個包存在問題。這裡我也碰到了bug:

@fatal error: GL/osmesa.h: 沒有此庫或目錄 #include <GL/osmesa.h>。

解決：#sudo apt-get install libosmesa6-dev 安裝缺少的GL/osmesa.h

4.修改源程式
由於參考的例子使用的環境適用kinetic深度相機或者baxter，所以在realsenseZR300需要改檔案使其找realsense_camera攝像頭驅動包對應釋出的話題名。將limemod涉及到的所有依賴包加到sublime，就可以大批量直接尋找要找的含舊話題名的檔案。
4.1首先檢視節點資訊 圖節點資訊

  #rosnode info /object_recognition_server
   檢視realsense給的例子需要訂閱的主題為：

   Subscriptions:

    * /camera/depth_registered/camera_info [unknown type]

    * /camera/depth_registered/image_raw [unknown type]

    * /camera/rgb/camera_info [sensor_msgs/CameraInfo]

    * /camera/rgb/image_rect_color [sensor_msgs/Image]

例子釋出了很多沒有用到的主題，用#rosnode cleanup清除無關此次專案開發的節點。

#rostopic echo /camera/depth_registered/sw_registered/camera_info
#rostopic echo /camera/depth_registered/image_raw
#rostopic echo /camera/rgb/camera_info
#rostopic echo /camera/rgb/image_rect_color

利用上面的命令即可檢視例程訂閱的資料，(當然檢視的前提是已經roslauch了realsense_camera zr300_nodelet_rgb)，這樣就可以看到如果例子realsense的例子跑起來，要用到的這四個話題是否有資料，是否成功釋出。

 用#rqt_graph 用節點圖形的方式來檢視執行的節點資訊。

 這樣我們核心就是要讓detection程式（也就是linemod例程）訂閱我們深度相機realsense釋出的話題。

4.2修改訂閱話題

4.2.1首先要裝sublime,當然這是程式開發必備的開發軟體，在這個工程中博主已經找到要修改的檔案所以可以跳過，但是便於之後開發sublime還是必備的。

注：在指定資料夾尋找需要的內容（在例子detection暗指的話題所在位置），但是用rosed命令還是預設gedit，所以要參考（http://wiki.lofarolabs.com/index.php/Using_rosed_to_Edit_Files_in_ROS），就可以直接用rosed命令直接關聯sublime。

4.2.2將用到的包linemod、ros、ork、core、msgs等（新安裝的包）拖入sublime查詢。

4.2.3搜尋linemod訂閱的的四個話題：（這裡根據之前zr300_nodelet_rgbd.launch節點發布的話題已經修改並註釋掉了）。

搜尋發現決定訂閱話題名的檔案位於linemod\conf\目錄下的detection.ros.ork檔案，將其修改：

在~/catkin_ws/src/linemod/conf目錄下的detection

  # rgb_frame_id: 'camera_rgb_optical_frame'

 # rgb_image_topic: '/camera/rgb/image_rect_color'

 # rgb_camera_info: '/camera/rgb/camera_info'

 # depth_frame_id: 'camera_depth_optical_frame'

 # depth_image_topic: '/camera/depth_registered/image_raw'

 # depth_camera_info: '/camera/depth_registered/camera_info'

修改為：

 #rgb_frame_id: 'camera_rgb_frame'

 #rgb_image_topic: '/camera/rgb/image_rect_color'

 #rgb_camera_info: '/camera/rgb/camera_info'

 #depth_frame_id: 'camera_depth_frame'

 #depth_image_topic: '/camera/depth_registered/sw_registered/image_rect'

 #depth_camera_info: '/camera/depth_registered/sw_registered/camera_info'

這樣linemod就可以找到rgb影象的資料以及D的資料，RGBD三維影象的資料就可以進行處理了。

5.匯入模型並訓練

5.1安裝CouchDB（用來建立物體辨識的工具）

 #sudo apt-get install couchdb

ORK tutorials為我們提供了一個可樂罐的3D模型（coke.stl）

 #git clone https://github.com/wg-perception/ork_tutorials
 #cm  (在src目錄下cmake)

5.2上傳coke物體及其mesh檔案

 #rosrun object_recog5.2nition_core object_add.py -n "coke " -d "A universal can of coke"

當然名字可以修改，接著在自己的資料庫檢視是否已經新增了該物體，將id記錄下來。以後想識別其他物體也在這個物體增加模型並訓練。

http://localhost:5984/_utils/database.html?object_recognition/_design/objects/_view/by_object_name

#rosrun object_recognition_core mesh_add.py <YOUR_OBJECT_ID> <path to ork_tutorials/data/coke.stl>
實際為：
#rosrun object_recognition_core mesh_add.py 0be612246c9b0a00baaa4adefb0009eb /home/yyang/catkin_ws/src/ork_tutorials/data/coke.stl --commit

這裡的obe…就是自己物體在CouchDB的id（博主是42d…）,5.2這兩條命令如果不能跑可以試試在後面加–commit。
6.訓練模型

#rosrun object_recognition_core training -c `rospack find object_recognition_linemod`/conf/training.ork

很快訓練完即可進行下一步detection

7.執行物體識別及檢測

#roslaunch realsense_camera zr300_nodelet_rgbd.launch
（如果之前已經launch過就不需要了，當然執行過也可以重新執行，配合下面的detection）
#rosrun object_recognition_core detection -c `rospack find object_recognition_linemod`/conf/detection.ros.ork
#rviz

結果如下：

這裡可樂罐對應的模型有一些偏置（白色罐和實際不重合對應，但是座標原點確實是實際可樂罐的位置），不知道源程式作者為什麼要這樣設定，之後再研究。

如果檢測到的話則會輸出：

注：如果出現不了點雲圖或者執行過程中沒有了點雲圖只有罐體以及座標就按下面：

#rosrun rqt_reconfigure rqt_reconfigure

在下拉選單中的driver中重新點選啟動enable_depth（打鉤）。類似下面這樣卡住的情形（在rosrun detection的時候出現）


8.測試

#rostopic list
可以看到ork釋出的話題資訊/recognized_object_array
#rostopic echo /recognized_object_array
即可實時檢視位置資訊
#rostopic type /recognized_object_array
使用type來檢視釋出資訊的型別，方便以後listener訂閱talker,處理該資訊傳送給機械臂處理執行抓取任務。

這裡就有了xyz以及物體識別的資訊了，位姿估計及位置計算。


用rqt可以檢視當前執行的全部節點 話題資訊（方形），還可以看活躍的節點。

可以看到當前執行的兩個例程中還有很多節點未使用，在以後的工作中逐步再處理應用。

另外，linemod演算法實時顯示影象並進行物體識別會卡，可以

#cd ~/catkin_ws目錄，將之前的debug版本換為release版本，重新編譯
#catkin_make -DCMAKE_BUILD_TYPE="Release"

並且，可以在system Monitor中（點最左上角圖示 搜sys…）可以檢視cpu佔用情況。

9.後續展望

此專案輸出：object_recognition_msgs/RecognizedObjectArray

之後針對該位置讀取位置資訊傳送給機械臂即可繼續下一步的工作。

#ipython
（ipython是python的進階版，比python更好用，需要安裝。以後用的多了再比較好用在哪兒）
#import rospy
#from object_recognition_msgs.msg import RecognizedObjectArray, RecognizedObject
將_msg包包含的資訊匯入，之後即可用例如
#msg=RecognizedObject()
來儲存到msg(例如輸入msg.pose.直接tap就可以檢視所有的資訊)並編寫listener python程式訂閱，得到資訊並處理。

基本用了兩個星期的時間做出來機器人的眼，這樣就可以識別物體以及得到其位姿，接下來就是對接機械臂，將點座標傳送給機械臂使其執行抓取任務。接下來的任務就是實踐釋出話題、訂閱話題，用python將結果記錄下來以便後續處理識別。另外還要總結內容、深入的理解一下linemod以及這個流程，包括match的三個引數、icp以及之前看到的icp+nss?進行後續的處理（物體重疊的時候）。