TechBridge 技術共筆部落格

var topics = ['Web前後端', '行動網路', '機器人/物聯網', '數據分析', '產品設計', 'etc.']

深入學習 lsd-slam - 5


前言

上次跟大家簡介了 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> // for formating strings
#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; // 相機 pose
ifstream fin("./pose.txt");
if (!fin)
{
cerr << "必須在有pose.txt的目錄下執行此程式" << endl;
return 1;
}
// 讀取彩色影像、深度影像以及對應的相機 pose
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 );
}
// 計算 point cloud 並接起來
// 指定相機內部參數
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
cout << "正在將影像轉換為 point cloud ..." << endl;
// 定義 point cloud 使用的格式:這邊用的是XYZRGB
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++ )
{
//使用內部參數與深度值算出相機座標系下的 point cloud
unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
if ( d==0 ) continue; // 深度為0表示沒有量到
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 );
}
}
//儲存 point cloud
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 的模式,如果你有正確執行,應該會看到如下面這張圖的效果:

lsd-slam-5-1

總結

這次很簡單地跟大家介紹了該怎麼使用相機的內部參數和外部參數,推得每個 pixel 在世界座標系中的位置,進而產生出 pointcloud,大家在有空時也不妨思考一下,我們是怎麼認識這個三維世界的,為何我們不需要知道每個 pointcloud 的確切位置就可以做好生活中的各項事情呢?

我們每天都在使用很多高級的演算法、完成很多複雜的事情,如果能將這些演算法實作於機器人,那就可以造出超級高級的機器人了,不過這一點也不容易就是了。

延伸閱讀

  1. Pinhole Camera:相機座標成像原理

關於作者:
@pojenlai 演算法工程師,對機器人跟電腦視覺有少許研究,最近在學習看清事物的本質與改進自己的觀念

喜歡我們的文章嗎?歡迎分享按讚給予我們支持和鼓勵!



TechBridge Weekly 技術週刊編輯團隊

TechBridge Weekly 技術週刊團隊是一群對用技術改變世界懷抱熱情的團隊。本技術共筆部落格初期專注於Web前後端、行動網路、機器人/物聯網、資料科學與產品設計等技術分享。This is TechBridge Weekly Team Tech Blog, which focus on web, mobile, robot, IoT, Data Science technology sharing.

關於我們 / 技術日報 / 技術週刊 / 粉絲專頁 / 訂閱RSS

Comments