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

群与李代数

群(G)是一种代数结构：集合(A)+运算(•):

群的特性：

1. 封闭性：

这里指的是a1,a2都属于集合A，那么a1•a2(a1、a2的任意运算结果)也属于集合A。

1. 结合律：

1. 幺元：

对于集合A中总存在一个元素，它跟集合中的任意元素做运算等于这个任意元素本身。比如说加法中的0，乘法中的1，

1. 逆：

对于集合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。

1. 双线性

有

这里相当于结合律。

1. 自反性

任取集合V中的一个子集X，这个子集与自己做二元运算的结果为0。李括号表示一种差异性，自己和自己的差异是0.

1. 雅可比等价

任取集合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)上的导数和扰动模型，通常情况下，扰动模型更为简洁实用。