2019-10-10

視覺SLAM之三維空間剛體運動描述

數(shù)學工具:旋轉(zhuǎn)矩陣,變換矩陣,四元數(shù),歐拉角

編程庫:Eigen庫

"旋轉(zhuǎn)矩陣"

既然是說剛體運動,首先剛體是什么?剛體指既有位置又有姿態(tài)(朝向)的物體;也就是說,要描述剛體的運動,就必須既要描述剛體的位置,還要描述剛體的姿態(tài)(朝向).

描述剛體運動,最重要的數(shù)學工具是線性代數(shù)中的點和向量.而要使用向量,必須要明白坐標和向量的關系.向量是線性空間中的一個對象,而描述這個對象的方法就是坐標.對于坐標和向量的關系,一個不準確的例子就是,把向量看做是線性空間的一個人,坐標是這個人的代號,那么,不管這個這個人叫什么代號,這個人本身沒有變化.為什么這個人有不同的代號?因為這個人在學校環(huán)境下,有學號,作為公民有身份證號等.回到線性代數(shù)的向量,不管坐標是什么,向量作為對象不會改變;為什么一個向量會有不同的坐標呢?是因為描述向量使用的參考系不同,也就是使用的基坐標不同,所以會有不同的坐標.比如:

$\vec{a} = \begin{bmatrix}e_1&e_2&e_3\end{bmatrix} \begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix} = a_1e_1 + a_2e_2 + a_3e_3$

其中 $\begin{bmatrix}e_1&e_2&e_3\end{bmatrix}$ 為基坐標, $\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}$ 為向量 $\vec{a}$ 在這組基坐標下的坐標.所以,描述向量之前,必須先指定基坐標.

既然同一個向量會因為不同的基坐標而產(chǎn)生不同的坐標,那么這些坐標之間有事嗎關系呢?

有的,假設有基坐標 $\begin{bmatrix}e_1&e_2&e_3\end{bmatrix}$ 組成的A坐標系和基坐標 $\begin{bmatrix}e_1'&e_2'&e_3'\end{bmatrix}$ 組成的B坐標系,那么向量 $\vec{a}$ 的坐標分別為 $\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}$ 和 $\begin{bmatrix}a_1'\\a_2'\\a_3'\end{bmatrix}$ .那么,因為向量 $\vec{a}$ 本身沒有變,因此有:

$\begin{bmatrix}e_1&e_2&e_3\end{bmatrix}\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix} = \begin{bmatrix}e_1'&e_2'&e_3'\end{bmatrix}\begin{bmatrix}a_1'\\a_2'\\a_3'\end{bmatrix}$

如果,在兩邊同時左乘 $\begin{bmatrix}e_1^T\\ e_2^T\\ e_3^T\end{bmatrix}$ 那么就得到了兩個坐標之間的關系:

$\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix} = \begin{bmatrix}e_1^T\\ e_2^T\\ e_3^T\end{bmatrix} \begin{bmatrix}e_1'&e_2'&e_3'\end{bmatrix}\begin{bmatrix}a_1'\\a_2'\\a_3'\end{bmatrix}=\begin{bmatrix}e_1^T e_1 e_1^T e_2 e_1^T e_3\\ e_2^T e_1 e_2^T e_2 e_2^T e_3\\e_3^T e_1 e_3^T e_2 e_3^T e_3\end{bmatrix}\begin{bmatrix}a_1'\\a_2'\\a_3'\end{bmatrix}\triangleq Ra'$

那么R就描述了兩個坐標之間的轉(zhuǎn)換,這個轉(zhuǎn)換只包括旋轉(zhuǎn),而不包括位移,因此R成為旋轉(zhuǎn)矩陣.

可以看到,旋轉(zhuǎn)矩陣是兩組基向量的向量積組成,通常情況下,基坐標都是單位正交向量,因此,旋轉(zhuǎn)矩陣一個單位正交矩陣,反過來,一個單位正交矩陣也可以作為一個旋轉(zhuǎn)矩陣,因此,把這一類矩陣有一個集合表示如下:

$SO(n) = \lbrace R \in \scr{R}^{n\times n} |\rm{R}R^T = \cal{I},\det(\rm{R}) = 1\rbrace$

$SO(n)$ 表示特殊正交群,其中 $n$ 表示維度.這樣的旋轉(zhuǎn)矩陣有個很好的特點.因為旋轉(zhuǎn)矩陣是單位正交矩陣,因此,如果一個旋轉(zhuǎn)矩陣為 $R$ ,那么和這個旋轉(zhuǎn)矩陣相反的旋轉(zhuǎn)矩陣就為 $R^T$ .

說完了旋轉(zhuǎn)矩陣,那么在SLAM中怎么應用呢?一般在SLAM中會定義一個世界坐標和一個相機坐標.世界坐標反應現(xiàn)實坐標,一般固定不懂,相機坐標是物體相對與相機的坐標,反應在相機拍攝的照片中.那么如何把圖片中的物體找到現(xiàn)實的位置呢?就是使用旋轉(zhuǎn)矩陣,通過世界坐標的基向量和相機坐標的基向量找到兩個坐標系下坐標的關系,然后就可以通過旋轉(zhuǎn)矩陣把照片中的物體的坐標找到現(xiàn)實的位置了.

齊次坐標和變換矩陣

旋轉(zhuǎn)矩陣就可以描述坐標在兩個坐標系中的關系了,但是這種關系只能描述矩陣的旋轉(zhuǎn),不能描述如果向量移動了,兩個坐標系中的坐標的關系.如果要加上位移,怎么表示呢?其實是需要在兩個坐標上同事加上位移就可以了,比如有一個位移 $t$ ,那么:

$a' = Ra + t$

這樣就完成了物體的運動在兩個坐標系下的轉(zhuǎn)換.

但是這種表示有一個缺點,就是這種表示并不是線性的,比如:

$b= R_1a + t_1$

$c = R_2 b + t_2$

那么從 $a$ 到 $c$ 的變換表示為:

$c = R_2(R_1a+t_1)+t_2$

這樣造成多次運動后,轉(zhuǎn)換變得復雜,為了解決這個問題,引入了變換矩陣和齊次坐標.

所謂的齊次坐標,就是在原來的坐標上增加一個維度,由于原來的坐標在增加那個維度的數(shù)值可以有無數(shù)種,同時,一個低維坐標在高維中也有無數(shù)中表示方法,所以,增加維度后的坐標稱為齊次坐標,例如:

$[1,1,1] \implies [2,2,2,2]$

那怎么把齊次坐標 $[x,y,z,w]^T$ 變回原來的坐標呢?如下:

$\tilde{x}=[x,y,z,w]^T=[x/w,y/w,z/w,1]^T$

我們把 $[x/w,y/w,z/w,1]^T$ 稱為非齊次坐標,這樣坐標的前三個值就是原來的坐標了.

說明了齊次坐標,怎么使用齊次坐標來克服之前坐標變換是非線性的問題呢?把原來的坐標增加一個維度,值為1,那么 $a' = Ra+t$ 可表示為:

$\begin{bmatrix}a' \\ 1 \end{bmatrix} = \begin{bmatrix}R&t\\0^T&1 \end{bmatrix}\begin{bmatrix}a\\1 \end{bmatrix} \triangleq T\begin{bmatrix} a\\1\end{bmatrix}$

那么 $\begin{bmatrix}R&t\\0^T&1 \end{bmatrix}$ 稱為變換矩陣,這個矩陣把旋轉(zhuǎn)和位移統(tǒng)一了起來.那么 $c = R_2(R_1a+t_1)+t_2$ 就可以表示為:

$\begin{bmatrix}c\\1\end{bmatrix}=T_2T_1\begin{bmatrix}a\\1\end{bmatrix}$

這樣,旋轉(zhuǎn)和位移的變換就稱為一個線性的運算.

同理,使用特殊歐式群表示這一類的變換矩陣為:

$SE(3)=\lbrace T=\begin{bmatrix} R & t\\0^T&1\end{bmatrix} \in \scr{R}^{4\times 4} | \rm{R} \in SO(3),t \in \scr{R}^3\rbrace$

那么一個變換矩陣的反變換可以表示為:

$T^{-1} = \begin{bmatrix} R^T & -R^Tt\\0^T&1\end{bmatrix}$

說完了向量的坐標之間的關系,那么如果要找到兩個基坐標之間的關系怎么做呢?原理是相同的,只要兩邊同時右乘坐標,就得到了兩組基之間的關系.

向量基本運算

接著說一下向量的基本運算.

向量運算包括數(shù)乘,向量加法,向量減法,向量內(nèi)積,向量外積等.其中,數(shù)乘和向量四則運算屬于基本內(nèi)容.這里只簡單說向量的內(nèi)積和外積.

向量外積

向量的外積也叫做點積/點乘,公式為:

$\vec{a} \cdot \vec引镊 = \vec{a}^T \vec爹梁 = \sum_{i=1}^n a_i b_i = |a| |b| \cos\langle a,b \rangle$

向量的內(nèi)積描述的是兩個向量之間的投影關系,內(nèi)積的結(jié)果是一個標量.

向量外積

向量的外積也叫作叉乘,公式為:

$\vec{a}\times\vec颜价 = \begin{bmatrix}\vec{i}&\vec{j}& \vec{k} \\ a_1 & a_2 &a_3\\b_1&b_2&b_3\end{bmatrix} = \begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}\vec妻率$

外積的結(jié)果是一個向量,這個向量垂直與 $\vec{a}$ 和 $\vec$ 組成的平面垂直;并且大小為:

$|\vec{a}\times\vec舅柜| = |\vec{a}||\vec瞒窒|\sin\langle\vec{a},\vec\rangle$

外積只在三維向量中有定義.外積可以用來表征兩個向量的旋轉(zhuǎn),因為外積的正負可以表征向量的旋轉(zhuǎn)方向,大小可以計算旋轉(zhuǎn)的角度大小.而內(nèi)積只能計算角度大小,不能表示旋轉(zhuǎn)的方向.

視覺SLAM十四講

?著作權歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者

人面猴
序言：七十年代末恋技，一起剝皮案震驚了整個濱河市拇舀，隨后出現(xiàn)的幾起案子，更是在濱河造成了極大的恐慌蜻底，老刑警劉巖骄崩，帶你破解...
沈念sama閱讀 219,490評論 6贊 508
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件，死亡現(xiàn)場離奇詭異薄辅，居然都是意外死亡要拂，警方通過查閱死者的電腦和手機，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 93,581評論 3贊 395
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進店門站楚，熙熙樓的掌柜王于貴愁眉苦臉地迎上來脱惰，“玉大人，你說我怎么就攤上這事窿春±唬” “怎么了？”我有些...
開封第一講書人閱讀 165,830評論 0贊 356
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵旧乞，是天一觀的道長蔚润。經(jīng)常有香客問我，道長尺栖，這世上最難降的妖魔是什么抽碌？我笑而不...
開封第一講書人閱讀 58,957評論 1贊 295
?港島之戀（遺憾婚禮）
正文為了忘掉前任，我火速辦了婚禮耐亏，結(jié)果婚禮上爷辙，老公的妹妹穿的比我還像新娘。我一直安慰自己褐墅，他們只是感情好痴颊，可當我...
茶點故事閱讀 67,974評論 6贊 393
惡毒庶女頂嫁案：這布局不是一般人想出來的
文/花漫我一把揭開白布赏迟。她就那樣靜靜地躺著，像睡著了一般蠢棱。火紅的嫁衣襯著肌膚如雪锌杀。梳的紋絲不亂的頭發(fā)上，一...
開封第一講書人閱讀 51,754評論 1贊 307
城市分裂傳說
那天泻仙，我揣著相機與錄音糕再，去河邊找鬼。笑死玉转，一個胖子當著我的面吹牛突想，可吹牛的內(nèi)容都是我干的。我是一名探鬼主播究抓，決...
沈念sama閱讀 40,464評論 3贊 420
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼猾担，長吁一口氣：“原來是場噩夢啊……” “哼！你這毒婦竟也來了刺下？” 一聲冷哼從身側(cè)響起绑嘹，我...
開封第一講書人閱讀 39,357評論 0贊 276
萬榮殺人案實錄
序言：老撾萬榮一對情侶失蹤，失蹤者是張志新（化名）和其女友劉穎橘茉，沒想到半個月后工腋，有當?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 45,847評論 1贊 317
?護林員之死
正文獨居荒郊野嶺守林人離奇死亡畅卓，尸身上長有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點故事閱讀 37,995評論 3贊 338
?白月光啟示錄
正文我和宋清朗相戀三年擅腰，在試婚紗的時候發(fā)現(xiàn)自己被綠了。大學時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片髓介。...
茶點故事閱讀 40,137評論 1贊 351
活死人
序言：一個原本活蹦亂跳的男人離奇死亡，死狀恐怖筋现，靈堂內(nèi)的尸體忽然破棺而出唐础，到底是詐尸還是另有隱情，我是刑警寧澤矾飞，帶...
沈念sama閱讀 35,819評論 5贊 346
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布一膨，位于F島的核電站，受9級特大地震影響洒沦，放射性物質(zhì)發(fā)生泄漏豹绪。R本人自食惡果不足惜，卻給世界環(huán)境...
茶點故事閱讀 41,482評論 3贊 331
男人毒藥：我在死后第九天來索命
文/蒙蒙一、第九天我趴在偏房一處隱蔽的房頂上張望瞒津。院中可真熱鬧蝉衣，春花似錦、人聲如沸巷蚪。這莊子的主人今日做“春日...
開封第一講書人閱讀 32,023評論 0贊 22
一樁弒父案，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽屁柏。三九已至啦膜，卻和暖如春，著一層夾襖步出監(jiān)牢的瞬間淌喻，已是汗流浹背僧家。一陣腳步聲響...
開封第一講書人閱讀 33,149評論 1贊 272
情欲美人皮
我被黑心中介騙來泰國打工，沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留裸删，地道東北人八拱。一個月前我還...
沈念sama閱讀 48,409評論 3贊 373
代替公主和親
正文我出身青樓，卻偏偏與公主長得像烁落，于是被迫代替她去往敵國和親乘粒。傳聞我的和親對象是個殘疾皇子，可洞房花燭夜當晚...
茶點故事閱讀 45,086評論 2贊 355

2019-10-10

視覺SLAM之三維空間剛體運動描述

"旋轉(zhuǎn)矩陣"

齊次坐標和變換矩陣

向量基本運算

推薦閱讀更多精彩內(nèi)容