TechBridge 技術共筆部落格

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

深入學習 LSD-SLAM - 4


前言

上次講了一些基本的理論介紹,但我們現在是希望學會如何實際寫程式讓機器人動起來,只有純數學的觀念跟機器人好像沒什麼關係,所以這次希望可以開始銜接到一個常用的工具 - Eigen。讓大家有動手實作、開始玩到東西的感覺。

為什麼要會 Eigen?

首先,Eigen 是一個 C++ 的 open source library,提供了許多線性代數運算相關的函式。

然後,我們這一系列是要介紹 LSD-SLAM,所以當然是因為 LSD-SLAM 也使用 Eigen 這個 library 來處理線性代數的運算,你可以在 lsd-slam 的 repository 裡搜尋 eigen,就會看到 以下的結果

另外,還有許多的工具,例如 g2osophus 也都有使用 Eigen,所以了解這個工具算是滿必要的。

安裝與小玩一下 Eigen

安裝滿簡單的,直接用下面這個指令就好:

1
sudo apt-get install libeigen3-dev

安裝完成之後,你應該可以在 /usr/include/eigen3 裡面找到相關的檔案,如果你進去看一下,就會看到很多酷炫的功能:

lsd-slam-4-1

接下來就讓我們來寫一個矩陣的小程式,首先先新增一個資料夾,開始寫 code:

1
2
mkdir test_eigen
vim eigenMatrix.cpp
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
#include <iostream>
using namespace std;

#include <Eigen/Core>

int main( int argc, char** argv )
{

// Eigen 中所有向量和矩陣都是Eigen::Matrix。它的前三個參數分別是:資料行態,row 數,col 數
// 宣告一個 2x3 的 float 矩陣
Eigen::Matrix<float, 2, 3> matrix_23;

// 初始化 matrix 的 element
matrix_23 << 1, 2, 3, 4, 5, 6;
// 輸出
cout << matrix_23 << endl;

// 用 () 來存取 matrix 中的 element
for (int i=0; i<2; i++) {
for (int j=0; j<3; j++)
cout<<matrix_23(i,j)<<"\t";
cout<<endl;
}

return 0;
}

寫完之後呢,還需要編譯,這邊也推薦大家使用 cmake,畢竟 ROS package 都使用 catkin_make,而 底層其實還是 cmake

1
vim CMakeLists.txt

裡面的內容是:

1
2
3
4
5
6
7
8
9
10
11
12
13
cmake_minimum_required( VERSION 2.8 )
project( eigenMatrix )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )

# Include Eigen headers
include_directories( "/usr/include/eigen3" )

# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )

add_executable( eigenMatrix eigenMatrix.cpp )

接下來編譯跟執行只需要:

1
2
3
cmake .
make
./eigenMatrix

你應該就可以看到如下的輸出了:

1
2
3
4
5
ros@ros-K401UB:~/code/eigen$ ./eigenMatrix 
1 2 3
4 5 6
1 2 3
4 5 6

眼尖的你應該會發現,資料夾裡面多了好多編譯過程中自動產生的文件,有點亂。所以,我們可以開一個 build 資料夾,讓這些中間產物都被放在這個資料夾,如果今天想要砍掉中間產物,砍掉這個資料夾就好了!這種方法會讓編譯過程變成:

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

相信有自己使用過 cmake 的讀者,對上面的步驟一定不陌生!

解線性方程組 & 座標轉換

線性代數也常常被用來解線性方程組或是來做座標轉換,如果你想要試著解看看,請參考這篇 解 least square 方法Space Transform

我們再來寫一個小程式來更熟悉 Eigen:

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
#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen/Geometry 提供了各种旋轉和平移的功能
#include <Eigen/Geometry>

int main ( int argc, char** argv )
{
// 3D 旋轉矩陣直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

// AngleAxis 就是旋轉向量,
//沿 Z 軸轉 45 度
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d( 0,0,1 ) );
cout .precision(3);
//用matrix()轉成矩陣輸出
cout << "rotation matrix =\n" << rotation_vector.matrix() << "\n" << endl;

// 接著我們產生大家熟悉的旋轉矩陣
rotation_matrix = rotation_vector.toRotationMatrix();

// Eigen 使用 typedef 提供了許多容易理解的資料型態,但 Vector3d 其實只是 Eigen::Matrix<double, 3, 3>
Eigen::Vector3d v ( 1,0,0 );

// 用旋轉向量來旋轉
Eigen::Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << "\n" << endl;

// 或者用旋轉矩陣旋轉
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << "\n" << endl;

// Euler angle: 可以從旋轉矩陣直接算出
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // roll pitch yaw
cout << "yaw pitch roll = " << euler_angles.transpose() << "\n" << endl;

// 四元數
// 可以直接把用 AngleAxis 來初始化
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout << "quaternion = \n" << q.coeffs() << "\n" << endl; // 順序是(x,y,z,w)
// 也可以用旋轉矩陣來初始化
q = Eigen::Quaterniond ( rotation_matrix );
cout << "quaternion = \n" << q.coeffs() << "\n" << endl;
// 使用四元數來旋轉向量(應該要跟之前用旋轉向量或旋轉矩陣的結果一樣)
v_rotated = q*v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << "\n" << endl;

return 0;
}

你執行成功之後應該可以看到下面的輸出:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
rotation matrix =
0.707 -0.707 0
0.707 0.707 0
0 0 1

(1,0,0) after rotation = 0.707 0.707 0

(1,0,0) after rotation = 0.707 0.707 0

yaw pitch roll = 0.785 -0 0

quaternion =
0
0
0.383
0.924

quaternion =
0
0
0.383
0.924

(1,0,0) after rotation = 0.707 0.707 0

總結

這次很簡單地跟大家介紹了 Eigen 這個 library,希望可以讓大家之後看到 Eigen 這個 library 會比較有親切跟熟悉的感覺,我們下回再見!

延伸閱讀

  1. 高翔大神書中 ch3 的程式碼
  2. Eigen 的官方 module 列表
  3. Eigen 的官方 Quick Reference(類似 cheatsheet)

關於作者:
@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