第二講 從圖像到點雲
本講中,我們將帶領讀者,編寫一個將圖像轉換爲點雲的程序。該程序是後期處理地圖的基礎。最簡單的點雲地圖即是把不同位置的點雲進行拼接得到的。
當我們使用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,該怎麼辦啊!
師兄:沒關係!你可以下載我們給你提供的數據。實際上就是下面兩張圖片啦!
小蘿蔔:怎麼深度圖是一團黑的呀!
師兄:請睜大眼睛仔細看!怎麼可能是黑的!
小蘿蔔:呃……可是確實是黑的啊!
師兄:對!這是由於畫面裏的物體離我們比較近,所以看上去比較黑。但是你實際去讀的話可是有數據的哦!
重要的備註:
- 這兩張圖來自於nyuv2數據集:http://cs.nyu.edu/~silberman/datasets/ 原圖格式是ppm和pgm的,被我轉成了png格式(否則博客園不讓傳……)。
- 你可以直接另存爲這兩個圖,也可以到我的git裏面獲取這兩個圖。
- 實際Kinect裏(或其他rgb-d相機裏)直接採到的RGB圖和深度圖可能會有些小問題:
- 有一些時差(約幾到十幾個毫秒)。這個時差的存在,會產生“RGB圖已經向右轉了,怎麼深度圖還沒轉”的感覺哦。
- 光圈中心未對齊。因爲深度畢竟是靠另一個相機獲取的,所以深度傳感器和彩色傳感器參數可能不一致。
- 深度圖裏有很多“洞”。因爲RGB-D相機不是萬能的,它有一個探測距離的限制啦!太遠或太近的東西都是看不見的呢。關於這些“洞”,我們暫時睜一隻眼閉一隻眼,不去理它。以後我們也可以靠雙邊bayes濾波器去填這些洞。但是!這是RGB-D相機本身的侷限性。軟件算法頂多給它修修補補,並不能完全彌補它的缺陷。
不過請你放心,在我們給出的這兩個圖中,都進行了預處理。你可以認爲“深度圖就是彩色圖裏每個像素距傳感器的距離”啦!
師兄:現在,我們要把這兩個圖轉成點雲啦,因爲計算每個像素的空間點位置,可是後面配準、拼圖等一系列事情的基礎呢。比如,在配準時,必須知道特徵點的3D位置呢,這時候就要用到我們這裏講到的知識啦!
小蘿蔔:聽起來很重要的樣子!
師兄:對!所以請讀者朋友務必掌握好這部分的內容啦!
從2D到3D(數學部分)
上面兩個圖像給出了機器人外部世界的一個局部的信息。假設這個世界由一個點雲來描述:X={x1,…,xn}X={x1,…,xn}. 其中每一個點呢,有 r,g,b,x,y,z一共6個分量,表示它們的顏色與空間位置。顏色方面,主要由彩色圖像記錄; 而空間位置,可以由圖像和相機模型、姿態一起計算出來。
,表示它們的顏色與空間位置。顏色方面,主要由彩色圖像記錄; 而空間位置,可以由圖像和相機模型、姿態一起計算出來。
對於常規相機,SLAM裏使用針孔相機模型(圖來自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf ):
從2D到3D (編程部分)
下面,我們來實現一個程序,完成從圖像到點雲的轉換。請在上一節講到的 代碼根目錄/src/ 文件夾中新建一個generatePointCloud.cpp文件:
1 touch src/generatePointCloud.cpp
小蘿蔔:師兄!一個工程裏可以有好幾個main函數麼?
師兄:對呀,cmake允許你自己定義編譯的過程。我們會把這個cpp也編譯成一個可執行的二進制,只要在cmakelists.txt裏作相應的更改便行了。
接下來,請在剛建的文件裏輸入下面的代碼。爲保證行文的連貫性,我們先給出完整的代碼,然後在重要的地方加以解釋。建議新手逐字自己敲一遍,你會掌握得更牢固。
// 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} )
然後,編譯新的工程:
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的警告,如果你編譯成功了,無視它即可。這是程序員的本能。