SLAM知識點整理

SLAM概述

 SLAM的全稱——Simultaneous Localization and Mapping(同時定位與地圖的構建)。它有三層含義，第一是進行機器人的姿態估計，第二是構建地圖，第三是同時進行這兩個事情。SLAM是一個雞生蛋、蛋生雞的問題，機器人構建地圖的時候需要知道自己目前所在的位置(定位)，同時在定位到自己的位置之後要進行下一步——走，需要看周圍的地圖。

在SLAM中，已知有2點，第一點機器人如何自我控制，如何運動。拿無人機舉例，無人機向前飛、向後飛、左轉、右轉、向上飛、向下飛等等。第二點是相應的觀測點。在已知這裏，我們想要的是地圖的一些特徵m={m1,m2,...,mk}，這裏就相當於一個構圖的過程，另外一塊就是機器人如何規劃自己的路徑，它朝哪個方向走可以避開一些障礙物，又或者是朝哪個路徑走可以最快速的離開一個區域。

它有兩個絕對無法改變的東西，第一個是機器人的位置，第二個是路標的位置，這兩個是定死的。它有一個相對量，雖然路標是定死的，但是對於機器人來說，它有一個相對位移測量，這個是在不停變化的，原因是機器人是在不停的運動的。

SLAM分爲激光雷達SLAM和視覺SLAM，視覺SLAM分爲以下幾步

1. 傳感器的讀取(Sensor Data)，在視覺SLAM中主要是使用相機或者叫做攝像頭來進行圖像信息的讀取和預處理。
2. 視覺里程計(Visual Odometry)，也稱爲前端。它的任務是估算相鄰圖像間相機的運動以及局部物體的樣子。視覺里程計簡稱VO。它所估算的是相鄰圖像，換句話說它只是估計一個局部的信息。
3. 迴環檢測(Loop Closure Detection)，也稱爲閉環檢測，這一部分是判斷機器人是否經過先前的位置。如果檢測到迴環之後就會把信息提供給後端進行處理。這一步的意義是我們不想讓機器人無時無刻都在處理所接受到的視覺信息，因爲機器人可能在某一刻回到了曾經來過的地方，那麼在這一時刻只要機器人意識到自己曾經來過，那麼它就可以調取之前的信息。如果沒有迴環檢測，那麼在構建地圖的時候會產生偏移，如果不把偏移拉回來，地圖會產生災難性的崩塌。
4. 後端優化(Optimization)，主要接受不同時刻視覺里程計測量的相機的位姿，位指位置，這樣我們就知道了相機大概的定位；姿指姿勢，這樣我們就知道了相機大概的狀態。後端優化除了接受視覺里程計所測量的輸入，同時還接受到閉環檢測的信息。接受到這兩個信息之後會對它們進行優化，最後得到全局一致的軌跡和地圖。
5. 建圖(Mapping)，會根據所估計到的軌跡，來建立與任務要求所對應的地圖，這個地圖可以是2D的拓撲地圖也可以是3D的度量地圖。3D地圖也可以分成多種多樣的形式，要看具體的要求。

相機的分類

相機一般可以分爲單目攝像頭，雙目攝像頭和深度攝像頭。雙目攝像頭表示有兩個一摸一樣的單目攝像頭在水平線上的排列。而RGB-Depth攝像頭除了可以採集到彩色圖片之外還可以讀出每個像素離相機的距離。

單目攝像頭就是採集到的普通照片，它通常會丟掉一個維度，就是我們所說的深度(距離)，比方說下面這張圖片

單目攝像頭無法分辨出冰淇淋筒上的是冰淇淋還是雲彩。因爲它無法給出遠近的信息，而遠近的信息在SLAM中是一個非常關鍵的信息。我們要通過距離這個信息估計出物體離我們的大致距離。

由於單目相機只是三維空間的二維投影，所以要真想恢復三維信息，就必須移動相機的視角。在相機的移動中就能估計出相機的運動，場景中物體的遠近和大小。

雙目相機和深度相機都是通過某種手段來測量物體離我們的距離，主要是爲了克服單目相機無法知道距離的缺點。如果知道距離，場景就可以通過單個圖像恢復出來。雙目攝像頭和深度攝像頭測量距離的原理是完全不同的，在雙目攝像頭中，兩個單目攝像頭的距離是已知的，稱爲基線(baseline)，我們就可以通過這個基線來估計空間中的位置。雙目攝像頭主要模仿的是人的兩隻眼睛。對於深度相機來說，它的特點是可以主動的發射紅外光或者是激光，接收反射，通過這個時間差來計算物體離相機的距離。雙目攝像頭是通過軟件的測量，對於深度攝像頭，它是通過物理的測量，深度攝像頭對於雙目攝像頭最大的好處就是可以節省大量的計算量，但是它的缺點就是如果所測量的物體可以吸收激光素，又或者是在室外的情況下，日照強烈，會干擾紅外光或者激光等。現在的深度相機會造成測量的範圍窄，噪聲大，容易受到日光的干擾，無法測量透射的物體等等問題。通常我們會把雙目攝像頭應用於室外和室內，而深度攝像頭應用於室內。

視覺里程計(前端)

它這裏關心的是相鄰圖像之間的運動，最簡單的情況是連續兩張圖像之間的運動關係，當然也可以是連續的N張圖片的運動關係，但它也是局部的圖像。在上圖中是在同一個場景拍攝的，右圖是左圖向左旋轉一定的角度。如果只是憑人眼，大概會對方向有一個敏感，但是對精確旋轉了多少度或者平移了多少距離的具體數字不是特別敏感。但是對於機器人來說，它必須精確的測量出運動的具體信息，這是非常關鍵的，因爲它要根據這些信息進行一個具體的運動。VO通過相鄰幀之間的估計相機運動來最後恢復場景的空間結構。VO只計算相鄰時刻的運動，並不計算整個過程的運動，和過去的信息沒有關聯。VO就像一條金魚一樣，只有短暫的記憶。

如果我們只要把相鄰幀的信息串聯起來是不是就可以解決定位和建圖的所有過程呢？如果僅僅憑前端來確定位置和建圖就會出現一個很嚴重的問題叫做累積漂移，視覺里程計僅僅能估算出相鄰幀之間的運動，而每次估計都會有一個誤差，對於任何精確的設備來說沒有辦法保證每一次測量都是非常準確的。爲了解決這個問題，所以需要後端優化和後端檢測。

閉環檢測

它主要解決位置估計隨時間漂移的問題。

在上圖中，右面的圖片是最終建立的3D地圖，是完全正確的。左面的這個圖片是當機器人走到某一時刻的時候，此時它會看到3個紅色的椅子，深紅色是三個椅子的真實位置，由於誤差的累積，它會發現這三個椅子在淺紅色的位置上。從理論上來講淺紅色的椅子應該和深紅色的椅子完全重合。如果沒有迴環估計這一塊，當機器人觀察到淺紅色的椅子後，會繼續往下走，它會認爲之前沒有來過這裏，是一個新的物體。它無法將這個地圖變成一個閉環的場景。迴環檢測所需要做的工作就是需要將這個偏差拉回來，使得淺紅色的椅子和深紅色的椅子重合，這樣就可以使機器人知道之前來過這裏，那麼就會把偏移拉回來，把地圖變成閉環。這個就是迴環檢測如何消除漂移。有一個非常簡單的消除漂移的方法就是給環境人爲的貼一些地標，比方說二維碼。機器人通過掃描會得到一些信息。或者是寫一些文字或者箭頭，通過文字識別或者是箭頭識別，機器人就知道以前來過這裏。這個在室內是可以這麼做的，但是如果是陌生環境或者是室外的話，我們更喜歡機器人能夠自我進行信息的處理。

迴環檢測所做的是全局一致，前端看的是局部。

後端優化

後端優化主要處理的是SLAM中的噪聲問題，說白了就是降噪。我們都希望數據是準確的，但沒有辦法保證每次接受到的信息完全沒有誤差(噪聲)。事實上再精確的傳感器都有一定的噪聲。再精確的傳感器在磁場較大的環境中也會失靈。我們除了要解決如何從圖像得知相機的運動之外，還得關心每次計算會有多大的誤差(噪聲)，噪聲是從上一個時刻往下傳遞，那麼我們又對當前的估計有多大的信心呢。後端優化所處理的就是如何從帶有噪聲的信息中估計出整個過程所處理的狀態。

地圖

地圖是對環境的描述，當然這個描述並不是唯一的，根據項目來的。根據項目的不同，最後所構建的地圖也不一樣。比方說下面這個拓撲地圖

家用掃地機器人所構建的就是這種二維的拓撲地圖。它只關心前面有沒有障礙物，它不關心它的上方有啥。有沒有來過這個地方清掃過。拓撲地圖更強調地圖中元素之間的聯通關係，但是對精確位置要求不高，所以它可以去掉大量的地圖細節。拓撲地圖是一種非常緊湊的地圖表達方式。

上圖是一個三維的度量地圖，它強調精確的表示地圖當中物體的位置關係，通常會有稀疏和稠密。稀疏地圖是對真實地圖進行一定的抽象，不需要表達所有的物體。比如右面這張圖，它所表達的是地圖中的椅子，桌子構建了出來，其他東西它就不關心了。這些具有代表意義的，我們稱之爲路標(landmark)。稠密地圖着重構建所有看到的東西。對於定位來說，稀疏地圖就已經夠了，但是對導航來說就完全不夠了，比如說這兩個物體之間有牆怎麼辦，我們不能讓無人機直接撞上去，此時就需要稠密地圖。稠密地圖會有更多的細分，https://www.bilibili.com/video/BV1Uh411976x?spm_id_from=333.337.search-card.all.click這裏就是一個三維重建的稠密地圖。

 SLAM基礎

點與向量

在二維座標系中，點的表示(x,y)；在三維座標系中，點的表示(x,y,z)

有關向量的內容可以參考線性代數整理 ，這裏不再贅述。這裏來看一下向量的外積，又叫叉乘

外積可以表示兩個向量的旋轉

a到b的旋轉可以由向量w來描述。在右手法則下，用右手的四指(除大拇指)，從a轉向b，大拇指的方向就是外積的方向，兩個向量的叉積依然是一個向量。大小由a、b的夾角決定。

import numpy as np

if __name__ == '__main__':

    a = np.array([-1, 0, 1])
    b = np.array([0, 2, 2])
    c = np.cross(a, b)
    print(c)
    print(np.linalg.norm(c))

運行結果

[-2  2 -2]
3.4641016151377544

剛體運動

剛體指的是一個幾何不變的物體。在三維空間中，把一個幾何不變物體做旋轉、平移的運動稱爲剛體運動。

座標系(e1,e2,e3)發生了旋轉，變成(e1',e2',e3')，向量a不動，那麼它的座標如何變化？

這裏如果要讓a與a'發生關係的話，可以用a'乘以一個旋轉矩陣，這個旋轉矩陣用R來表示

旋轉矩陣R的必要條件：行列式爲1的正交矩陣。，SO(n)是特殊正交羣：Sepcial Orthogonal Group。SO(n)這個集合是由n維空間的旋轉矩陣組成。n爲3的時候就是三維空間的旋轉。旋轉矩陣可以描述相機的運動。旋轉矩陣爲正交陣，它的逆(即轉置)描述了一個相反的旋轉：

行列式爲1的正交矩陣實際上就是一個標準正交方陣，即一個正交單位矩陣。因爲是標準正交矩陣，所以逆=轉置。具體可以參考線性代數整理(二) 中的標準正交矩陣。行列式的內容可以參考線性代數整理(三)

世界座標系中的向量a，經過一次旋轉(R)和平移(t)後，得到了a':

a'=Ra+t        t爲平移向量

空間變換：

這裏表示相機從a的位置不斷旋轉加平移到e的位置的一系列過程，但是我們的相機總是在不斷的變換位置的，如果按照這種算法非常浪費時間，我們來看一個這樣的推導過程

我們給三維向量a的末尾增加一個維度，並且這個維度的值爲1，那麼滿足

這個T叫做變換矩陣(Transform Matrix)，稱爲齊次座標。對於這個四維向量，我們相當於把旋轉和平移寫在了一個矩陣裏面，使得整個關係變成了一個線性關係。齊次座標中，某個點的每個分量同乘以一個非零常數後，仍表示同一個點。

比如說

這裏表示的是齊次座標旋轉和平移後到，那麼和的關係就只是和兩個變換矩陣相乘，大大簡化了計算量。之後以來表示，這裏的a和b不再是三維空間向量，而表示一個齊次座標。如果相機不停的發生連續的變化，那麼每次就乘以相應的變換矩陣T。

之前有一個SO(3)，SO(3)僅僅考慮了旋轉就是R，SE(3)的全稱爲Special Euclidean Group，它考慮的是變換矩陣。變換矩陣有一個特殊的結構，它的左上角是3*3的旋轉矩陣，右側是3*1的平移向量，左下角爲0向量，右下角爲1。這種矩陣又稱爲特殊歐式羣，它跟SO(3)一樣，求解矩陣的逆表示一個反向的變化。

這個就是剛體的運動，其中包含了SO(3)和SE(3)，SO(3)考慮的是剛體的旋轉，我們更多考慮的是SE(3)，SE(3)表示的是加上了平移向量之後的平移矩陣。

SO(3)與SE(3)對加法是不封閉的，也就是說兩個旋轉矩陣相加不再是一個旋轉矩陣；兩個變換矩陣相加也不再是一個變換矩陣。

SO(3)與SE(3)對乘法是封閉的，也就是說兩個旋轉矩陣相乘依然是一個旋轉矩陣，相當於做了兩次旋轉；兩個變換矩陣相乘也依然是一個變換矩陣，相當於做了兩次位置的變換。

旋轉向量和歐拉角

假設有一個旋轉軸爲n，角度爲θ的旋轉，顯然它對應的旋轉向量爲θn。

羅德里格斯公式：

通過羅德里格斯公式，我們可以把旋轉向量寫成旋轉矩陣，其中n^表示一個對角矩陣，格式如下

反過來，從一個旋轉矩陣到旋轉向量的轉換。對於轉角θ：

其中角度：

軸：

對於轉軸n，相當於旋轉軸上的向量在旋轉後不發生改變，因此轉軸n其實是矩陣R的特徵值爲1對應的特徵向量。有關特徵值和特徵向量的內容可以參考線性代數整理(三) 中的特徵值和特徵向量。

歐拉角

偏航-俯仰-滾轉：    yaw-roll-pitch    ->  z-x-y。在上圖，飛機就是一個剛體，這裏的x、y、z對應着歐拉座標系的三個軸，表示着剛體沿某一個軸來進行旋轉，沿x軸(飛機頭指向的前方)旋轉稱爲俯仰角，沿y軸(機身向右的水平方向)旋轉稱爲滾轉角，沿z軸(機身向下的垂直方向)旋轉稱爲偏航角。它們統稱爲歐拉角。

歐拉角一個最大的問題就是萬向鎖(Gimbal Lock)，在俯仰角爲正負90度的時候，第一次旋轉和第三次旋轉將會使用同一個軸，這個時候系統就會丟掉一個自由度，由三次旋轉變成兩次旋轉，通常我們稱之爲奇異型問題。如果我們使用三維實數來表達三維旋轉的話就會碰到奇異型問題，遇到這種問題，歐拉角並不適合差值和迭代，所以我們在描述剛體的運動的時候如果僅僅考慮三維的話，碰到萬向鎖問題，相當於死機。通常我們會把三維變到四維。

四元數

一個四元數q擁有一個實部和三個虛部：    

三個虛部：

我們還可以i換一種寫法，用一個標量和一個向量來表達四元數：

我們已經用了兩種方式來表示旋轉，第一種方式就是旋轉矩陣，旋轉矩陣R就是一個3*3=9個數字來描述一個三個自由度的旋轉，其實這個是非常麻煩的，用9個數來表述三個自由度的旋轉是有冗餘性的。第二種表示方式就是歐拉角和旋轉向量，歐拉角是一種比較好的表示旋轉的方式，但是它存在一個萬向鎖的問題，這個就是不太好的，所以這裏引出了四元數。

• 四元數的運算

這裏有兩個四元數和，運算如下

加法：

減法；

乘法：

也可以換一種寫法

這裏的是一個叉乘，四元數的乘法不滿足交換律。

共軛：

模長：

逆：

Python實現：

import math

class Quaternion:

    def __init__(self, s, x, y, z):
        self.s = s
        self.x = x
        self.y = y
        self.z = z
        self.vector = [x, y, z]
        self.all = [s, x, y, z]

    def __str__(self):
        op = [" ", "i ", "j ", "k "]
        q = self.all.copy()
        result = ""
        for i in range(4):
            if q[i] < -1e-8 or q[i] > 1e-8:
                result = result + str(round(q[i], 4)) + op[i]
        if result == "":
            return "0"
        else:
            return result

    def __add__(self, other):
        q = self.all.copy()
        for i in range(4):
            q[i] += other.all[i]
        return Quaternion(q[0], q[1], q[2], q[3])

    def __sub__(self, other):
        q = self.all.copy()
        for i in range(4):
            q[i] -= other.all[i]
        return Quaternion(q[0], q[1], q[2], q[3])

    def __mul__(self, other):
        q = self.all.copy()
        p = other.all.copy()
        s = q[0] * p[0] - q[1] * p[1] - q[2] * p[2] - q[3] * p[3]
        x = q[0] * p[1] + q[1] * p[0] + q[2] * p[3] - q[3] * p[2]
        y = q[0] * p[2] - q[1] * p[3] + q[2] * p[0] + q[3] * p[1]
        z = q[0] * p[3] + q[1] * p[2] - q[2] * p[1] + q[3] * p[0]
        return Quaternion(s, x, y, z)

    def divide(self, other):
        """
        右除
        """
        return self * other.inverse()

    def modpow(self):
        """模的平方"""
        q = self.all
        return sum([i**2 for i in q])

    def mod(self):
        """模"""
        return math.sqrt(self.modpow())

    def conj(self):
        """共軛"""
        q = self.all.copy()
        return Quaternion(q[0], -q[1], -q[2], -q[3])

    def inverse(self):
        """求逆"""
        q = self.conj().all
        mod = self.modpow()
        for i in range(4):
            q[i] /= mod
        return Quaternion(q[0], q[1], q[2], q[3])

if __name__ == '__main__':

    a = Quaternion(1, 1, 2, 3)
    b = Quaternion(1, 1, 2, 3)
    c = a.conj()
    d = b.inverse()
    e = a.modpow()
    f = a.divide(b)
    g = f * b

    print(c)
    print(d)
    print(e)
    print(f)
    print(g)
    print(a, b)

運行結果

1 -1i -2j -3k 
0.0667 -0.0667i -0.1333j -0.2k 
15
1.0 
1.0 1.0i 2.0j 3.0k 
1 1i 2j 3k  1 1i 2j 3k 

• 四元數到歐拉角的變換

三維空間的單位向量,某個旋轉是繞單位向量n進行了角度爲θ的旋轉，該旋轉的四元數形式爲：

反之如果知道q，也可以算出θ和n

之前我們知道一個三維點p旋轉到p'，只需要乘以一個旋轉矩陣R，則有

對於這個三維點p可以寫成一個四元數爲

因爲可以用q來表示一個旋轉

則p旋轉到p‘可以寫成

現在我們來寫一個比較簡單的旋轉

n = [1, 0, 0]
seta = math.radians(180)
p = [1, 1, 0]
p = Quaternion(0, p[0], p[1], p[2])
q = Quaternion(math.cos(seta / 2), n[0] * math.sin(seta / 2), n[1] * math.sin(seta / 2), n[2] * math.sin(seta / 2))
p1 = q * p * q.inverse()
print(p1)

運行結果

1.0i -1.0j 

這段代碼表示將p點(1,1,0)沿着x軸旋轉180度，結果變成了(1,-1,0)

四元數也可以根據相應的R來進行表示

相反如果知道R也可以反過來求q

如果當接近於0的時候，剩下三個分量會非常大，這個時候就會非常不穩定，此時我們會考慮用其他的方式進行轉換。不管是四元數、旋轉矩陣還是軸都可以描述同一個旋轉，只不過是旋轉矩陣比較直觀，但是旋轉矩陣有3*3=9個值來表示三維的旋轉，比較費事；所以我們引出了歐拉角，歐拉角最大問題就是萬向鎖的問題，所以此時我們才引出了四元數。不管是四元數、旋轉矩陣還是歐拉角它都可以描述同一旋轉，實際應用中應當選擇最爲方便的方式，並不是說必須得用哪一種方式，比如說四元數或者旋轉矩陣。

SLAM的運動模型：

這裏x表示機器人的位置，是運動傳感器的輸入，是噪聲。所謂運動的就是k-1時刻到k時刻，機器人的位置是如何變化的。我們可以通過一個旋轉矩陣和一個平移向量或者是加一個變換矩陣然後一步一步的往下估計。

SLAM的觀測模型：

這裏z代表觀測數據，是機器人的位置，是路標點的絕對座標，是噪聲。當我們拿到一些數據的時候，比如說z觀測數據，u機器人的運動，我們要如何求解x的定位問題和y的建圖問題。求解這兩個問題，我們統稱爲狀態估計問題。

比方說有一個初始的值(x,y)，在下一個時刻有一個估計的值，這個是有誤差的，我們要消滅誤差，我們就需要把誤差最小化，我們該通過什麼樣的手段最快的把誤差最小化呢？通常我們會利用沿着導數的方向，使得誤差函數收斂到最小值。根據導數的定義，我們知道旋轉矩陣的導數

這裏有一個問題就是我們知道兩個旋轉矩陣相加並不是一個旋轉矩陣，也就是這裏的，此時我們就無法對R做導數。鑑於這個問題，就需要引入一個新的數學意義來解決，這就是李代數。

項目實戰：Eigen

我這裏是mac系統，所以安裝Eigen的方式爲

brew install eigen

在CMakeLists.txt中添加

find_package(Eigen3 REQUIRED)
include_directories("/usr/local/include/eigen3")

C++中矩陣，向量的使用

#include <iostream>
#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>

//std指標準庫
using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc,char** argv) {
//    Eigen中所有向量和矩陣都說是Matrix,這是一個2行3列的float矩陣
    Matrix<float,2,3> matrix_23;
//    內置類型，底層依舊是Matrix，這是一個3維向量
    Vector3d v_3d;
    Matrix<float,3,1> vd_3d;
//    Matrix<double,3,3>,並初始化爲0
    Matrix3d matrix_33 = Matrix3d::Zero();
//    不確定矩陣大小
    Matrix<double,Dynamic,Dynamic> matrix_dynamic;
    MatrixXd matrix_x;
//    輸入數據，初始化
    matrix_23 << 1,2,3,4,5,6;
    cout << "matrix_23:\n" << matrix_23 << endl;
    cout << "Each element in the matrix_23:\n";
    for (int i = 0; i < 2; ++i) {
        for (int j = 0; j < 3; ++j) {
            cout << matrix_23(i,j) << "\t";
        }
        cout << endl;
    }
    cout << "---------------" << endl;
    v_3d << 1,2,3;
    vd_3d << 4,5,6;
//    Eigen不能混合不同類型，需要顯示轉換，這是一個矩陣與向量相乘，結果爲一個向量
    Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;
    cout << "---------------" << endl;
    Matrix<float,2,1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;
    cout << "---------------" << endl;
//    隨機數矩陣
    matrix_33 = Matrix3d::Random();
    cout << matrix_33 << endl;
    cout << "---------------" << endl;
//    轉置
    cout << matrix_33.transpose() << endl;
    cout << "---------------" << endl;
//    各元素和
    cout << matrix_33.sum() << endl;
    cout << "---------------" << endl;
//    跡
    cout << matrix_33.trace() << endl;
    cout << "---------------" << endl;
//    數乘
    cout << 10 * matrix_33 << endl;
    cout << "---------------" << endl;
//    逆
    cout << matrix_33.inverse() << endl;
    cout << "---------------" << endl;
//    行列式
    cout << matrix_33.determinant() << endl;
    cout << "---------------" << endl;
//    特徵值和特徵矩陣
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << eigen_solver.eigenvalues() << endl;
    cout << eigen_solver.eigenvectors() << endl;

    Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
    matrix_NN = MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Matrix<double,MATRIX_SIZE,1> v_Nd;
    v_Nd = MatrixXd::Random(MATRIX_SIZE,1);
//    計時
    clock_t time_stt = clock();
//    直接求逆
    Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
    cout << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    time_stt = clock();
//    通常用矩陣分解來求，例如QR分解，速度會快很多
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    return 0;
}

運行結果

matrix_23:
1 2 3
4 5 6
Each element in the matrix_23:
1	2	3	
4	5	6	
---------------
14
32
---------------
32
77
---------------
 -0.999984 -0.0826997  -0.905911
 -0.736924  0.0655345   0.357729
  0.511211  -0.562082   0.358593
---------------
 -0.999984  -0.736924   0.511211
-0.0826997  0.0655345  -0.562082
 -0.905911   0.357729   0.358593
---------------
-1.99453
---------------
-0.575857
---------------
 -9.99984 -0.826997  -9.05911
 -7.36924  0.655345   3.57729
  5.11211  -5.62082   3.58593
---------------
 -0.370316  -0.888554 -0.0491136
 -0.737309  -0.172358   -1.69072
 -0.627782   0.996559   0.208558
---------------
-0.606436
---------------
0.281738
0.548924
   2.378
 0.213003 -0.511493  0.832469
 0.972431  0.193748  -0.12977
-0.094913   0.83716   0.53866
3.366ms
3.167ms

矩陣微分

矩陣函數

這裏的X是一個m*n的矩陣，F也是一個矩陣，它代表的意思是把m*n個數映射成p*q個數。這裏的m、n、p、q都是大於等於1的整數，當m或者n其中一個爲1的時候，就相當於把一個向量映射成了一個矩陣。當p或者q其中一個爲1的時候，就相當於把一個矩陣映射成一個向量。如果這兩對都有其中一個數爲1的話，那就相當於把一個向量映射成了另一個向量。如果這四個數都是1的話就相當於一個一元函數。如果p、q都是1，m、n其中有一個爲1，就相當於把一個向量映射成了一個常數，此時就相當於多元函數。所以矩陣函數就是對於一元函數和多元函數的推廣形式。

這是一個向量x，包含了x1、x2，我們將其映射成了另一個向量。

矩陣函數其實就是針對多個向量內的每一個分量，每一個分量是一個幾元函數。矩陣由向量組成，矩陣和向量就是數據的排列方式不同而已，所有m*n的矩陣都可以轉化爲nm*1的向量。因此這裏只需要知道向量值微分即可，矩陣微分無非是其重新排列組合而已。

向量的微分算符

我們先將一個向量映射成一個實數

我們以來說明，這裏矩陣A是一個n*n的方陣。我們知道要滿足矩陣相乘，則x必定是一個n*1的列向量，是一個1*n的行向量，則(1*n)*(n*n)*(n*1)=(1*n)*(n*1)=1*1，最終結果是一個實數。我們將其展開有

這裏是依據向量相乘的內積=各個分量相乘再累加。我們對第i個分量進行求導

如果對每一個分量都去算一次偏導數是很麻煩的，下面是矩陣微分的求導法則

行列式的求導法則

跡函數

在線性代數中，一個n×n矩陣A的主對角線（從左上方至右下方的對角線）上各個元素的總和被稱爲矩陣A的跡（或跡數），一般記作Tr(A)。矩陣的跡表示的是特徵值的和，它不隨基的變化而變化。通常，這種特性可以用來定義線性算子的軌跡。（注意：跡是對方陣而言的）。

常用性質：

1. 跡是所有主對角元素的和
2. 跡是所有特徵值的和
3. 某些時候也利用來求跡。
4. ，跡是滿足線性映射的
7. 矩陣乘積的跡

羣與李代數

羣(G)是一種代數結構：集合(A)+運算(•):

羣的特性：

1. 封閉性：這裏指的是a1,a2都屬於集合A，那麼a1•a2(a1、a2的任意運算結果)也屬於集合A。
2. 結合律：
3. 幺元：對於集合A中總存在一個元素，它跟集合中的任意元素做運算等於這個任意元素本身。比如說加法中的0，乘法中的1，
4. 逆：對於集合A中的任意一個元素，總能夠找到該元素的逆，兩者做運算等於幺元。比如說加法中的相反數，它們相加總等於0；乘法中的倒數，它們相乘總等於1。

一般線性羣GL(n)，指n*n的可逆矩陣，它們對矩陣乘法成羣。特殊正交羣SO(n)，也就是所謂的旋轉矩陣羣，如SO(2)和SO(3)。特殊歐式羣SE(n)，也就是所謂的變換矩陣羣，如SE(2)和SE(3)。

李羣

具有連續(光滑)性質的羣，這裏的連續性保證了羣可以求導；李羣既是羣也是流形。SO(n)羣和SE(n)羣在實數空間上是連續的，但是它們只有定義良好的乘法，沒有加法，所以難以進行取極限、求導等操作。流形的意思就是說空間中的光滑的運動，表達一種連續性，中間沒有中斷。

任何李羣都有一個對應的李代數，反之亦然。李代數就是李羣單位元素的正切空間，其實就是導數形式，但並非導數。我們如何從李羣過度到李代數，因爲通過李代數可以求導，但是李羣不能求導。

我們依然來看一下三維旋轉矩陣R構成的特殊正交羣：

R是某個相機的旋轉，它會隨時間連續地變化，即爲時間的函數:R(t)

這裏的I是一個單位矩陣。

求導(R的上面有一個點)：

整理：

反對稱矩陣的概念，對於一個向量，它的反對稱矩陣

如果不考慮矩陣中的負號，那麼它就是一個對稱矩陣。叉乘就是一個反對稱矩陣，從向量->反對稱矩陣可以寫爲：

反對稱矩陣->向量

是反對稱矩陣，找到一個三維向量與之對應，則有

等式兩邊右乘R(t)，由於R爲正交陣，則有

這就相當於旋轉矩陣R求一次導數就相當於左乘一個反對稱矩陣。

令=0，且R(0)=I，即初始時刻還沒有發生旋轉，將R(t)在處一階泰勒展開(有關泰勒展開可以參考高等數學整理 中的泰勒公式定義)：

可見ø反映了一階導數性質，它位於正切空間(targent space,就是一階導數)上。現在我們可以得到這麼三個式子

這裏我們假設在附近ø不變，將第二個式子代入第一個式子，有

它代表着一個函數求導等於該函數本身乘以一個常數，這讓我們想起指數函數求導

由於R(0)=I，則有

這就意味着

它表示旋轉矩陣R，它與另外一個反對稱矩陣ø，通過指數關係發生了聯繫。換句話說當我們知道一個旋轉矩陣R的時候，存在一個向量ø，R和ø滿足一個指數的關係。現在我們想知道的是ø到底是什麼，長什麼樣子？ø是一個矩陣，那麼矩陣的指數又怎麼求？

現在我們來稍微總結一下，每一個李羣都有與之對應的李代數；李代數描述了李羣單位元附近的正切空間性質。

李代數

李代數由一個集合V，一個數域F和一個二元運算[,]組成。這個二元運算，我們稱之爲李括號，相對於羣中的二元運算，李代數中的二元運算表示了兩個元素的差異。如果它們滿足以下幾條性質，稱爲一個李代數，記作。

1. 封閉性在集合V中任取兩個子集X、Y，對這兩個子集進行二元運算的結果依然屬於集合V。
2. 雙線性有這裏相當於結合律。
3. 自反性任取集合V中的一個子集X，這個子集與自己做二元運算的結果爲0。李括號表示一種差異性，自己和自己的差異是0.
4. 雅可比等價任取集合V中的三個子集X、Y、Z，這個三個子集兩兩做二元運算再與第三個子集做二元運算，將三種可能相加的結果爲0.

我們來舉一個例子，三維空間向量進行叉積運算，構成了李代數。

這裏的ø是一個三維向量，ø1、ø2、ø3是ø中的三個元素，Φ是ø的反對稱矩陣，李括號的意義就是

它表示兩個三維向量做李代數的二元運算，即爲它們的反對稱矩陣分別相乘(順序不同)再相減後恢復成向量。這裏是一個旋轉矩陣的李代數。

在變換矩陣中

這裏的ε是一個6維的向量，前三維的ρ作爲平移，後三維的ø作爲旋轉，這裏的不是一個反對稱矩陣，表示的是將6維向量轉換成一個4維的矩陣。李括號的意義就是

它表示兩個六維向量做李代數的二元運算，它們分別六維轉四維矩陣後分別相乘(順序不同)再相減後恢復成向量。

指數與對數映射

我們知道旋轉空間可以表示爲這個公式。李羣到李代數是一種指數映射，對於指數映射同樣可以做泰勒展開

那麼so(3)的泰勒展開

由於ø是向量，定義其方向a和模長θ，則有

a的性質如下：

1. 長度爲1的單位向量。

那麼so(3)的泰勒展開可以推導如下

之前我們給出過旋轉矩陣的羅德里格斯公式

我們會發現它們倆非常像

這就說明李代數so(3)的物理意義就是旋轉向量。我們知道從李羣到李代數是一種指數關係，那麼反過來從李代數到李羣就是一種對數關係，如果定義對數映射，我們也能把SO(3)中的元素對應到so(3)中

但是如果我們真的這麼去做的話，計算量會非常大。我們需要換種思路

變換矩陣的指數映射，se(3)到SE(3)

小結：

李代數的求導與擾動模型

首先李羣沒有辦法做加法，導數無從定義

我們使用李代數的一大動機就是進行優化，而優化的過程中求導是非常必要的信息。

當我們在旋轉矩陣中完成乘法之後，那麼它對應的李代數so(3)上面發生了什麼改變呢？換句話說對一個李代數求導，然後把它折射回李羣上，它的解決方法如下

1. 先利用李代數上加法定義李羣元素的導數；
2. 再使用指數映射和對數映射完成變換關係。

現在的問題是，當我們在李代數上做加法的時候它是否等於在李羣上做乘法。

在使用標量的情況下，上式明顯成立：

但是在這裏的ø^爲矩陣，上式不成立：

這裏我們引入BCH(Baker-Campbell-Hausdorff)公式，考慮SO(3)上的李代數(對數/指數)：

它表示當向量是一個小量的時候，它乘以一個係數，就被保留了下來；相反當是一個小量的時候，它乘以一個係數，就被保留了下來。而這個係數就是雅可比係數。

在我們SLAM中會使用的是左雅可比。假設對某一個旋轉R，它對應的李代數爲ø，當我們給它左乘一個微小的旋轉∆R,那麼對應的李代數就會對應一個微小的變換∆ø，在李羣上對應的就是∆R*R，在李代數上就是

即爲加法上相差左雅可比的逆。反之

它表示李代數上進行小量加法時，相當於李羣上左(右)乘一個帶左(右)雅可比的量。

同理對於SE(3)

擾動模型

在SLAM中，我們要估計相機的位姿，該位姿是由李羣上的旋轉矩陣或者是變換矩陣來描述的。機器人目前的位姿爲T，觀察點座標爲p，產生了一個觀測數據z，則有

這裏w爲噪聲，它也是一個實際的測量值。它表示的是觀測的數據z就是當前的機器人通過通過一系列的變換，即爲變換矩陣T挪到觀察點的位置p，那麼誤差就爲

若有N個這樣的路標點和觀測，對於機器人的位姿估計就相當於找一個最優的T,使得整體誤差最小化。

此時我們就需要計算目標函數J關於變換矩陣的導數，它有兩種解決方案：

1. 用李代數表示姿態，然後對根據李代數加法來對李代數求導。
2. 對李羣左乘或右乘微小擾動，然後對該擾動求導，稱爲左擾動或右擾動模型。

假設我們對一個空間點p進行了旋轉，得到了Rp，旋轉後點的座標相對於旋轉的導數，不嚴謹地記爲：，由於旋轉矩陣R沒有加法，導數無從定義。所以我們轉爲對應的李代數來進行求導。這個求導也是剛纔的兩種方法來進行：

1. 對R對應的李代數加上小量，求相對於小量的變化率(導數模型)；
2. 對R左乘或右乘一個小量，求相對於小量的李代數的變化率(擾動模型).

導數模型：

這裏的第一行是導數的原始定義，第二行是BCH的線形近似，第三行是泰勒展開去掉高階後的近似，第四行和第五行是通過反對稱矩陣的負號做叉積。這個計算量比較大。

擾動模型(左乘)：左乘小量，令其李代數爲零

‘

擾動模型和導數模型相比，少了一個雅可比的計算，所以擾動模型更爲實用。

SE(3)擾動模型(左乘)：

利用BCH線性近似，可以推導so(3)與se(3)上的導數和擾動模型，通常情況下，擾動模型更爲簡潔實用。