2019-10-10

视觉SLAM之三维空间刚体运动描述

数学工具:旋转矩阵,变换矩阵,四元数,欧拉角

编程库:Eigen库

"旋转矩阵"

既然是说刚体运动,首先刚体是什么?刚体指既有位置又有姿态(朝向)的物体;也就是说,要描述刚体的运动,就必须既要描述刚体的位置,还要描述刚体的姿态(朝向).

描述刚体运动,最重要的数学工具是线性代数中的点和向量.而要使用向量,必须要明白坐标和向量的关系.向量是线性空间中的一个对象,而描述这个对象的方法就是坐标.对于坐标和向量的关系,一个不准确的例子就是,把向量看做是线性空间的一个人,坐标是这个人的代号,那么,不管这个这个人叫什么代号,这个人本身没有变化.为什么这个人有不同的代号?因为这个人在学校环境下,有学号,作为公民有身份证号等.回到线性代数的向量,不管坐标是什么,向量作为对象不会改变;为什么一个向量会有不同的坐标呢?是因为描述向量使用的参考系不同,也就是使用的基坐标不同,所以会有不同的坐标.比如:

$\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}$ 在这组基坐标下的坐标.所以,描述向量之前,必须先指定基坐标.

既然同一个向量会因为不同的基坐标而产生不同的坐标,那么这些坐标之间有事吗关系呢?

有的,假设有基坐标 $\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就描述了两个坐标之间的转换,这个转换只包括旋转,而不包括位移,因此R成为旋转矩阵.

可以看到,旋转矩阵是两组基向量的向量积组成,通常情况下,基坐标都是单位正交向量,因此,旋转矩阵一个单位正交矩阵,反过来,一个单位正交矩阵也可以作为一个旋转矩阵,因此,把这一类矩阵有一个集合表示如下:

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

$SO(n)$ 表示特殊正交群,其中 $n$ 表示维度.这样的旋转矩阵有个很好的特点.因为旋转矩阵是单位正交矩阵,因此,如果一个旋转矩阵为 $R$ ,那么和这个旋转矩阵相反的旋转矩阵就为 $R^T$ .

说完了旋转矩阵,那么在SLAM中怎么应用呢?一般在SLAM中会定义一个世界坐标和一个相机坐标.世界坐标反应现实坐标,一般固定不懂,相机坐标是物体相对与相机的坐标,反应在相机拍摄的照片中.那么如何把图片中的物体找到现实的位置呢?就是使用旋转矩阵,通过世界坐标的基向量和相机坐标的基向量找到两个坐标系下坐标的关系,然后就可以通过旋转矩阵把照片中的物体的坐标找到现实的位置了.

齐次坐标和变换矩阵

旋转矩阵就可以描述坐标在两个坐标系中的关系了,但是这种关系只能描述矩阵的旋转,不能描述如果向量移动了,两个坐标系中的坐标的关系.如果要加上位移,怎么表示呢?其实是需要在两个坐标上同事加上位移就可以了,比如有一个位移 $t$ ,那么:

$a' = Ra + t$

这样就完成了物体的运动在两个坐标系下的转换.

但是这种表示有一个缺点,就是这种表示并不是线性的,比如:

$b= R_1a + t_1$

$c = R_2 b + t_2$

那么从 $a$ 到 $c$ 的变换表示为:

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

这样造成多次运动后,转换变得复杂,为了解决这个问题,引入了变换矩阵和齐次坐标.

所谓的齐次坐标,就是在原来的坐标上增加一个维度,由于原来的坐标在增加那个维度的数值可以有无数种,同时,一个低维坐标在高维中也有无数中表示方法,所以,增加维度后的坐标称为齐次坐标,例如:

$[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}$ 称为变换矩阵,这个矩阵把旋转和位移统一了起来.那么 $c = R_2(R_1a+t_1)+t_2$ 就可以表示为:

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

这样,旋转和位移的变换就称为一个线性的运算.

同理,使用特殊欧式群表示这一类的变换矩阵为:

$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}$

说完了向量的坐标之间的关系,那么如果要找到两个基坐标之间的关系怎么做呢?原理是相同的,只要两边同时右乘坐标,就得到了两组基之间的关系.

向量基本运算

接着说一下向量的基本运算.

向量运算包括数乘,向量加法,向量减法,向量内积,向量外积等.其中,数乘和向量四则运算属于基本内容.这里只简单说向量的内积和外积.

向量外积

向量的外积也叫做点积/点乘,公式为:

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

向量的内积描述的是两个向量之间的投影关系,内积的结果是一个标量.

向量外积

向量的外积也叫作叉乘,公式为:

$\vec{a}\times\vec{b} = \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{b}$

外积的结果是一个向量,这个向量垂直与 $\vec{a}$ 和 $\vec{b}$ 组成的平面垂直;并且大小为:

$|\vec{a}\times\vec{b}| = |\vec{a}||\vec{b}|\sin\langle\vec{a},\vec{b}\rangle$

外积只在三维向量中有定义.外积可以用来表征两个向量的旋转,因为外积的正负可以表征向量的旋转方向,大小可以计算旋转的角度大小.而内积只能计算角度大小,不能表示旋转的方向.

视觉SLAM十四讲

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 194,491评论 5赞 459
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 81,856评论 2赞 371
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 141,745评论 0赞 319
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 52,196评论 1赞 263
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 61,073评论 4赞 355
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 46,112评论 1赞 272
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 36,531评论 3赞 381
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 35,215评论 0赞 253
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 39,485评论 1赞 290
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 34,578评论 2赞 309
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 36,356评论 1赞 326
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 32,215评论 3赞 312
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 37,583评论 3赞 299
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 28,898评论 0赞 17
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 30,174评论 1赞 250
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 41,497评论 2赞 341
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 40,697评论 2赞 335

2019-10-10

视觉SLAM之三维空间刚体运动描述

"旋转矩阵"

齐次坐标和变换矩阵

向量基本运算

推荐阅读更多精彩内容