前言 上次跟大家簡介了 Eigen 這個可以用來做線性代數運算的 open source library，讓大家有動手實作、開始玩到東西的感覺。今天要延續動手實作的精神，跟大家介紹一下該怎麼把幾張 RGB-D 影像拼接成 point cloud，接下來就讓我們一起玩玩吧！
參考資料來源 這次的 data（包含 RGB 影像跟 Depth Map） 等等都是來自於 slambook 的 ch5 ，有這些資料真的超讚的，不然自己光要產生這些資料就得花一些時間，還要有硬體，比較難快速地上手。
核心概念 我們擁有的 data 是 RGB-D 感測器在 5 個不同的 pose 底下拍到的影像，利用相機的內部參數將一組 RGB-D 影像中的像素對應回 3D 相機座標系下的 point cloud，然後再利用各組圖的 camera pose，將各組 point cloud 對應到同一個世界座標系下，就能組合出地圖。
其中 pose.txt 儲存的格式是平移向量加上旋轉四元數：
$[x, y, z, q_x, q_y, q_z, q_w]$
如果你對內部參數和外部參數的概念不熟，網路上有頗多資源，個人覺得延伸閱讀 1 的講解算是十分清楚的，推薦去看看！
實作 基本函式庫安裝 首先要安裝 OpenCV2，因為待會寫程式需要讀取影像，因為 OpenCV 也是一個 cmake project，步驟比較複雜一些，可以直接看看官方安裝頁面 。
然後是安裝 PCL：
1 2 3 sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl sudo apt-get update sudo apt-get install libpcl-all
程式碼 接下來就是程式碼啦，其實你可以先跑起來再慢慢理解：
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 #include <iostream> #include <fstream> using namespace std;#include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <Eigen/Geometry> #include <boost/format.hpp> #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/pcl_visualizer.h> int main ( int argc, char ** argv ) vector<cv::Mat> colorImgs, depthImgs; vector<Eigen::Isometry3d> poses; ifstream fin ("./pose.txt" ) ; if (!fin) { cerr << "必須在有pose.txt的目錄下執行此程式" << endl; return 1 ; } for ( int i=0 ; i<5 ; i++ ) { boost::format fmt ( "./%s/%d.%s" ) ; colorImgs.push_back ( cv::imread ( (fmt%"color" %(i+1 )%"png" ).str () )); depthImgs.push_back ( cv::imread ( (fmt%"depth" %(i+1 )%"pgm" ).str (), -1 )); double data[7 ] = {0 }; for ( auto & d:data ) fin>>d; Eigen::Quaterniond q ( data[6 ], data[3 ], data[4 ], data[5 ] ) ; Eigen::Isometry3d T (q) ; T.pretranslate ( Eigen::Vector3d ( data[0 ], data[1 ], data[2 ] )); poses.push_back ( T ); } double cx = 325.5 ; double cy = 253.5 ; double fx = 518.0 ; double fy = 519.0 ; double depthScale = 1000.0 ; cout << "正在將影像轉換為 point cloud ..." << endl; typedef pcl::PointXYZRGB PointT; typedef pcl::PointCloud<PointT> PointCloud; PointCloud::Ptr pointCloud ( new PointCloud ) ; for ( int i=0 ; i<5 ; i++ ) { cv::Mat color = colorImgs[i]; cv::Mat depth = depthImgs[i]; Eigen::Isometry3d T = poses[i]; for ( int v=0 ; v<color.rows; v++ ) for ( int u=0 ; u<color.cols; u++ ) { unsigned int d = depth.ptr <unsigned short > ( v )[u]; if ( d==0 ) continue ; Eigen::Vector3d point; point[2 ] = double (d)/depthScale; point[0 ] = (u-cx)*point[2 ]/fx; point[1 ] = (v-cy)*point[2 ]/fy; Eigen::Vector3d pointWorld = T*point; PointT p ; p.x = pointWorld[0 ]; p.y = pointWorld[1 ]; p.z = pointWorld[2 ]; p.b = color.data[ v*color.step+u*color.channels () ]; p.g = color.data[ v*color.step+u*color.channels ()+1 ]; p.r = color.data[ v*color.step+u*color.channels ()+2 ]; pointCloud->points.push_back ( p ); } } pointCloud->is_dense = false ; cout << "There are total " << pointCloud->size () << " points in the map.pcd." <<endl; pcl::io::savePCDFileBinary ("map.pcd" , *pointCloud ); return 0 ; }
編譯與執行 接下來就是編譯啦：
1 2 3 4 5 6 mkdir build cd build cmake .. make mv joinMap ../ cd ..
編譯完就可以執行並觀察產生的 map.pcd 檔了。
1 2 ./joinMap pcd_viewer map.pcd
一開始開啟 pcd_viewer ，會看到所有的 pointcloud都是同一個顏色的，要按 5 才能進入 RGB 的模式，如果你有正確執行，應該會看到如下面這張圖的效果：
總結 這次很簡單地跟大家介紹了該怎麼使用相機的內部參數和外部參數，推得每個 pixel 在世界座標系中的位置，進而產生出 pointcloud，大家在有空時也不妨思考一下，我們是怎麼認識這個三維世界的，為何我們不需要知道每個 pointcloud 的確切位置就可以做好生活中的各項事情呢？
我們每天都在使用很多高級的演算法、完成很多複雜的事情，如果能將這些演算法實作於機器人，那就可以造出超級高級的機器人了，不過這一點也不容易就是了。
延伸閱讀
Pinhole Camera：相機座標成像原理
關於作者：@pojenlai 演算法工程師，對機器人跟電腦視覺有少許研究，最近在學習看清事物的本質與改進自己的觀念
喜歡我們的文章嗎？歡迎分享按讚給予我們支持和鼓勵！
留言討論