本講中，我們將帶領讀者，編寫一個將3D圖像轉換為3D點雲的程序。該程序是后期處理地圖的基礎。最簡單的點雲地圖即是把不同位置的點雲進行拼接得到的。
當我們使用RGB-D相機時，會從相機里讀到兩種數據：彩色圖像和深度圖像。
由於沒有相機，我們采用的深度圖和RGB圖。我們要把這兩個圖轉成點雲啦，因為計算每個像素的空間點位置，可是后面配准、拼圖等一系列事情的基礎呢。比如，在配准時，必須知道特征點的3D位置呢，這時候就要用到我們這里講到的知識啦！
-----------------------------------------------------------------------------------------------------------------------------------------------------
從2D到3D（數學部分）
如果相機發生了位移和旋轉，那么只要對這些點進行位移和旋轉操作即可。
-----------------------------------------------------------------------------------------------------------------------------------------------------
從2D到3D（編程部分）
下面，我們來實現一個程序，完成從圖像到點雲的轉換。請在上一節講到的 代碼根目錄/src/ 文件夾中新建一個generatePointCloud.cpp文件：
// C++ 標准庫 #include <iostream> #include <string> using namespace std; // OpenCV 庫 #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> // PCL 庫 #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> // 定義點雲類型 typedef pcl::PointXYZRGBA PointT; typedef pcl::PointCloud<PointT> PointCloud; // 相機內參 const double camera_factor = 1000; const double camera_cx = 325.5; const double camera_cy = 253.5; const double camera_fx = 518.0; const double camera_fy = 519.0; // 主函數 int main( int argc, char** argv ) { // 讀取./data/rgb.png和./data/depth.png，並轉化為點雲 // 圖像矩陣 cv::Mat rgb, depth; // 使用cv::imread()來讀取圖像 // API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread rgb = cv::imread( "./data/rgb.png" ); // rgb 圖像是8UC3的彩色圖像 // depth 是16UC1的單通道圖像，注意flags設置-1,表示讀取原始數據不做任何修改 depth = cv::imread( "./data/depth.png", -1 ); // 點雲變量 // 使用智能指針，創建一個空點雲。這種指針用完會自動釋放。 PointCloud::Ptr cloud ( new PointCloud ); // 遍歷深度圖 for (int m = 0; m < depth.rows; m++) for (int n=0; n < depth.cols; n++) { // 獲取深度圖中(m,n)處的值 ushort d = depth.ptr<ushort>(m)[n]; // d 可能沒有值，若如此，跳過此點 if (d == 0) continue; // d 存在值，則向點雲增加一個點 PointT p; // 計算這個點的空間坐標 p.z = double(d) / camera_factor; p.x = (n - camera_cx) * p.z / camera_fx; p.y = (m - camera_cy) * p.z / camera_fy; // 從rgb圖像中獲取它的顏色 // rgb是三通道的BGR格式圖，所以按下面的順序獲取顏色 p.b = rgb.ptr<uchar>(m)[n*3]; p.g = rgb.ptr<uchar>(m)[n*3+1]; p.r = rgb.ptr<uchar>(m)[n*3+2]; // 把p加入到點雲中 cloud->points.push_back( p ); } // 設置並保存點雲 cloud->height = 1; cloud->width = cloud->points.size(); cout<<"point cloud size = "<<cloud->points.size()<<endl; cloud->is_dense = false; pcl::io::savePCDFile( "./pointcloud.pcd", *cloud ); // 清除數據並退出 cloud->points.clear(); cout<<"Point cloud saved."<<endl; return 0; }
程序運行需要數據。請把上面的那兩個圖存放在 代碼根目錄/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} )
2.編譯並運行
1 cd build 2 cmake .. #..代表代碼根目錄 /home/louis/slam 3 make 4 cd ..
如果編譯通過，就可在bin目錄下找到新寫的二進制：generate_pointcloud 運行它：
bin/generate_pointcloud
即可在data目錄下生成點雲文件。現在，你肯定希望查看一下新生成的點雲了。如果已經安裝了pcl，就可以通過：
1 cd ../data
2 pcl_viewer pointcloud.pcd
太開心了！！終於有結果了！