第二講 從影象到點雲

　　本講中，我們將帶領讀者，編寫一個將影象轉換為點雲的程式。該程式是後期處理地圖的基礎。最簡單的點雲地圖即是把不同位置的點雲進行拼接得到的。

　　當我們使用RGB-D相機時，會從相機裡讀到兩種資料：彩色影象和深度影象。如果你有Kinect和ros，可以執行：

1 roslaunch openni_launch openni.launch

　　使Kinect工作。隨後，如果PC連線上了Kinect，彩色影象與深度影象就會發布在 /camera/rgb/image_color 和 /camera/depth_registered/image_raw 中。你可以通過：

1
 
 rosrun image_view image_view image:=/camera/rgb/image_color

　　來顯示彩色影象。或者，你也可以在Rviz裡看到影象與點雲的視覺化資料。

　　小蘿蔔：可是師兄！我現在手邊沒有Kinect，該怎麼辦啊！

　　師兄：沒關係！你可以下載我們給你提供的資料。實際上就是下面兩張圖片啦！

　　小蘿蔔：怎麼深度圖是一團黑的呀！

　　師兄：請睜大眼睛仔細看！怎麼可能是黑的！

　　小蘿蔔：呃……可是確實是黑的啊！

　　師兄：對！這是由於畫面裡的物體離我們比較近，所以看上去比較黑。但是你實際去讀的話可是有資料的哦！

　　重要的備註：

1. 這兩張圖來自於nyuv2資料集：http://cs.nyu.edu/~silberman/datasets/ 原圖格式是ppm和pgm的，被我轉成了png格式（否則部落格園不讓傳……）。
2. 你可以直接另存為這兩個圖，也可以到我的git裡面獲取這兩個圖。
3. 實際Kinect裡（或其他rgb-d相機裡）直接採到的RGB圖和深度圖可能會有些小問題：
   • 有一些時差（約幾到十幾個毫秒）。這個時差的存在，會產生“RGB圖已經向右轉了，怎麼深度圖還沒轉”的感覺哦。
   • 光圈中心未對齊。因為深度畢竟是靠另一個相機獲取的，所以深度感測器和彩色感測器引數可能不一致。
   • 深度圖裡有很多“洞”。因為RGB-D相機不是萬能的，它有一個探測距離的限制啦！太遠或太近的東西都是看不見的呢。關於這些“洞”，我們暫時睜一隻眼閉一隻眼，不去理它。以後我們也可以靠雙邊bayes濾波器去填這些洞。但是！這是RGB-D相機本身的侷限性。軟體演算法頂多給它修修補補，並不能完全彌補它的缺陷。

　　不過請你放心，在我們給出的這兩個圖中，都進行了預處理。你可以認為“深度圖就是彩色圖裡每個畫素距感測器的距離”啦！

　　師兄：現在，我們要把這兩個圖轉成點雲啦，因為計算每個畫素的空間點位置，可是後面配準、拼圖等一系列事情的基礎呢。比如，在配準時，必須知道特徵點的3D位置呢，這時候就要用到我們這裡講到的知識啦！

　　小蘿蔔：聽起來很重要的樣子！

　　師兄：對！所以請讀者朋友務必掌握好這部分的內容啦！

從2D到3D（數學部分）

　　上面兩個影象給出了機器人外部世界的一個區域性的資訊。假設這個世界由一個點雲來描述：$X=\{ x_1, \ldots, x_n \}$. 其中每一個點呢，有 $r,g,b,x,y,z$一共6個分量，表示它們的顏色與空間位置。顏色方面，主要由彩色影象記錄； 而空間位置，可以由影象和相機模型、姿態一起計算出來。

　　對於常規相機，SLAM裡使用針孔相機模型（圖來自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf ）：

$$ u = \frac{ x \cdot f_x }{z} + c_x $$

$$ v = \frac{ y \cdot f_y }{z} + c_y $$

$$ d = z \cdot s $$

　　其中，$f_x, f_y$指相機在$x,y$兩個軸上的焦距，$c_x, c_y$指相機的光圈中心，$s$指深度圖的縮放因子。

　　小蘿蔔：好暈啊！突然冒出這麼多個變數！

　　師兄：別急啊，這已經是很簡單的模型了，等你熟悉了就不覺得複雜了。 

　　這個公式是從$(x,y,z)$推到$(u,v,d)$的。反之，我們也可以把它寫成已知$(u,v,d)$，推導$(x,y,z)$的方式。請讀者自己推導一下。

　　不，還是我們來推導吧……公式是這樣的：

 $$ z = d/s $$

$$ x = (u-c_x) \cdot z/f_x $$

$$ y = (v-c_y) \cdot z/ f_y $$

　　怎麼樣，是不是很簡單呢？事實上根據這個公式就可以構建點雲啦。

　　通常，我們會把$f_x, f_y, c_x, c_y$這四個引數定義為相機的內參矩陣$C$，也就是相機做好之後就不會變的引數。相機的內參可以用很多方法來標定，詳細的步驟比較繁瑣，我們這裡就不提了。給定內參之後呢，每個點的空間位置與畫素座標就可以用簡單的矩陣模型來描述了：

$$ s \cdot \left[ \begin{array}{l} u \\ v \\ 1 \end{array} \right] = C \cdot \left( R \cdot \left[ \begin{array}{l} x \\ y \\ z \end{array} \right] +t  \right)$$

　　其中，$R$和$t$是相機的姿態。$R$代表旋轉矩陣，$t$代表位移向量。因為我們現在做的是單幅點雲，認為相機沒有旋轉和平移。所以，把$R$設成單位矩陣$I$，把$t$設成了零。$s$是scaling factor，即深度圖裡給的資料與實際距離的比例。由於深度圖給的都是short (mm單位)，$s$通常為1000。

　　小蘿蔔：於是就有了上面那個$(u,v,d)$轉$(x,y,z)$的公式？

　　師兄：對！真聰明！如果相機發生了位移和旋轉，那麼只要對這些點進行位移和旋轉操作即可。

從2D到3D （程式設計部分）

　　下面，我們來實現一個程式，完成從影象到點雲的轉換。請在上一節講到的 程式碼根目錄/src/ 資料夾中新建一個generatePointCloud.cpp檔案：

1 touch src/generatePointCloud.cpp

　　小蘿蔔：師兄！一個工程裡可以有好幾個main函式麼？

　　師兄：對呀，cmake允許你自己定義編譯的過程。我們會把這個cpp也編譯成一個可執行的二進位制，只要在cmakelists.txt裡作相應的更改便行了。

　　接下來，請在剛建的檔案裡輸入下面的程式碼。為保證行文的連貫性，我們先給出完整的程式碼，然後在重要的地方加以解釋。建議新手逐字自己敲一遍，你會掌握得更牢固。

 1 // C++ 標準庫
 2 #include <iostream>
 3 #include <string>
 4 using namespace std;
 5 
 6 // OpenCV 庫
 7 #include <opencv2/core/core.hpp>
 8 #include <opencv2/highgui/highgui.hpp>
 9 
10 // PCL 庫
11 #include <pcl/io/pcd_io.h>
12 #include <pcl/point_types.h>
13 
14 // 定義點雲型別
15 typedef pcl::PointXYZRGBA PointT;
16 typedef pcl::PointCloud<PointT> PointCloud; 
17 
18 // 相機內參
19 const double camera_factor = 1000;
20 const double camera_cx = 325.5;
21 const double camera_cy = 253.5;
22 const double camera_fx = 518.0;
23 const double camera_fy = 519.0;
24 
25 // 主函式 
26 int main( int argc, char** argv )
27 {
28     // 讀取./data/rgb.png和./data/depth.png，並轉化為點雲
29 
30     // 影象矩陣
31     cv::Mat rgb, depth;
32     // 使用cv::imread()來讀取影象
33     // API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
34     rgb = cv::imread( "./data/rgb.png" );
35     // rgb 影象是8UC3的彩色影象
36     // depth 是16UC1的單通道影象，注意flags設定-1,表示讀取原始資料不做任何修改
37     depth = cv::imread( "./data/depth.png", -1 );
38 
39     // 點雲變數
40     // 使用智慧指標，建立一個空點雲。這種指標用完會自動釋放。
41     PointCloud::Ptr cloud ( new PointCloud );
42     // 遍歷深度圖
43     for (int m = 0; m < depth.rows; m++)
44         for (int n=0; n < depth.cols; n++)
45         {
46             // 獲取深度圖中(m,n)處的值
47             ushort d = depth.ptr<ushort>(m)[n];
48             // d 可能沒有值，若如此，跳過此點
49             if (d == 0)
50                 continue;
51             // d 存在值，則向點雲增加一個點
52             PointT p;
53 
54             // 計算這個點的空間座標
55             p.z = double(d) / camera_factor;
56             p.x = (n - camera_cx) * p.z / camera_fx;
57             p.y = (m - camera_cy) * p.z / camera_fy;
58             
59             // 從rgb影象中獲取它的顏色
60             // rgb是三通道的BGR格式圖，所以按下面的順序獲取顏色
61             p.b = rgb.ptr<uchar>(m)[n*3];
62             p.g = rgb.ptr<uchar>(m)[n*3+1];
63             p.r = rgb.ptr<uchar>(m)[n*3+2];
64 
65             // 把p加入到點雲中
66             cloud->points.push_back( p );
67         }
68     // 設定並儲存點雲
69     cloud->height = 1;
70     cloud->width = cloud->points.size();
71     cout<<"point cloud size = "<<cloud->points.size()<<endl;
72     cloud->is_dense = false;
73     pcl::io::savePCDFile( "./pointcloud.pcd", *cloud );
74     // 清除資料並退出
75     cloud->points.clear();
76     cout<<"Point cloud saved."<<endl;
77     return 0;
78 }

　　程式執行需要資料。請把上面的那兩個圖存放在 程式碼根目錄/data 下（沒有這個資料夾就新建一個）。

　　我們使用OpenCV的imread函式讀取圖片。在OpenCV2裡，影象是以矩陣(cv::MAt)作為基本的資料結構。Mat結構既可以幫你管理記憶體、畫素資訊，還支援一些常見的矩陣運算，是非常方便的結構。彩色影象含有R,G,B三個通道，每個通道佔8個bit（也就是unsigned char），故稱為8UC3（8位unsigend char, 3通道）結構。而深度圖則是單通道的影象，每個畫素由16個bit組成（也就是C++裡的unsigned short），畫素的值代表該點離感測器的距離。通常1000的值代表1米，所以我們把camera_factor設定成1000. 這樣，深度圖裡每個畫素點的讀數除以1000，就是它離你的真實距離了。

　　接下來，我們按照“先列後行”的順序，遍歷了整張深度圖。在這個雙重迴圈中：

1 for (int m = 0; m < depth.rows; m++)
2      for (int n=0; n < depth.cols; n++)

　　m指影象的行，n是影象的列。它和空間點的座標系關係是這樣的：

　　深度圖第m行，第n行的資料可以使用depth.ptr<ushort>(m) [n]來獲取。其中，cv::Mat的ptr函式會返回指向該影象第m行資料的頭指標。然後加上位移n後，這個指標指向的資料就是我們需要讀取的資料啦。

　　計算三維點座標的公式我們已經給出過了，程式碼裡原封不動地實現了一遍。我們根據這個公式，新增了一個空間點，並放入了點雲中。最後，把整個點雲端儲存為 ./data/pointcloud.pcd 檔案。

編譯並執行

　　最後，我們在src/CMakeLists.txt里加入幾行程式碼，告訴編譯器我們希望編譯這個程式。請在此檔案中加入以下幾行：

# 增加PCL庫的依賴
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )

# 增加opencv的依賴
FIND_PACKAGE( OpenCV REQUIRED )

# 新增標頭檔案和庫檔案
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp )
TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS} 
    ${PCL_LIBRARIES} )

　　然後，編譯新的工程：

1 cd build
2 cmake ..
3 make
4 cd ..

　　如果編譯通過，就可在bin目錄下找到新寫的二進位制：generate_pointcloud 執行它：

bin/generate_pointcloud

　　即可在data目錄下生成點雲檔案。現在，你肯定希望檢視一下新生成的點雲了。如果已經安裝了pcl，就可以通過：

1 pcl_viewer pointcloud.pcd

　　來檢視新生成的點雲。

課後作業

　　本講中，我們實現了一個從2D影象到3D點雲的轉換程式。下一講，我們將探討影象的特徵點提取與配準。配準過程中，我們需要計算2D影象特徵點的空間位置。因此，請你編寫一個頭檔案與一個原始檔，實現一個point2dTo3d函式。請在標頭檔案裡寫這個函式的宣告，原始檔裡給出它的實現，並在cmake中把它編譯成一個叫做slam_base的庫。（你需要考慮如何定義一個比較好的介面。）這樣一來，今後當我們需要計算它時，就只需呼叫這個函式就可以了。

　　小蘿蔔：師兄！這個作業看起來有些難度啊！

　　師兄：是呀，不能把讀者想的太簡單嘛。

　　最後呢，本節用到的原始碼仍然可以從我的git裡下載到。讀者的鼓勵就是對我最好的支援！

TIPS:

• 如果你開啟點雲，只看到紅綠藍三個方塊，請按R重置視角。剛才你是站在原點盯著座標軸看呢。
• 如果點雲沒有顏色，請按5顯示顏色。
• cmake過程可能有PCL的警告，如果你編譯成功了，無視它即可。這是程式設計師的本能。