SLAM公式引出、推导和理解 1-2

1.7 李群李代数

群（Group）是一种集合加上另一种运算的代数结构。把集合记作 $A$ ，运算记作 $\cdot$，群可以记作 $G=(A,\cdot )$。李群是指具有连续（光滑）性质的群。
三维旋转矩阵和变换矩阵对应着李群这一概念。$$\begin{array}{rl}& SO(3)=\left\{R\in {\mathbb{R}}^{3\times 3}|R{R}^T=I,det(R)=1\right\}\\ & SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\right\}\end{array}$$因为对于旋转矩阵或者变换矩阵来说，对加法是不封闭的，$$R_1+R_2\notin SO(3），T_1+T_2\notin SE(3)$$这一点对于进一步优化（求导等问题）来说，是很棘手的问题。但是对于乘法是封闭的。$$R_1{R}_2\in SO(3），T_1{T}_2\in SE(3)$$
由于对任意旋转矩阵 $R$ ，满足$$R{R}^T=I$$这一性质。$R$ 是某相机随时间变换的旋转，即为时间的函数：$R(t)$。有$$R(t)R(t{)}^T=I$$对时间求导得$$\begin{array}{r}\dot{R}(t)R(t{)}^T+R(t)\dot{R}(t{)}^T=0\\ \dot{R}(t)R(t{)}^T=-(\dot{R}(t)R(t{)}^T{)}^T\end{array}$$可以看出 $\dot{R}(t)R(t{)}^T$ 是反对称矩阵(anti-symmetric matrix)。反对称矩阵就是向量和矩阵的映射关系。
由式子(1.1.3)可知，对任意反对称矩阵，可以找到与之一一对应得向量。把这个运算用 ${}^{\vee }$ 表示($A$ 为矩阵，$a$ 为向量，用符号 ${}^{\wedge }$ 表示从向量到矩阵的转换，符号 ${}^{\vee }$ 表示从矩阵到向量的转换)：$$a^{\wedge }=A=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]，A^{\vee }=a$$于是，找到三维向量 $\varphi (t)\in {\mathbb{R}}^3$与之对应。$$\begin{array}{rl}\dot{R}(t)R(t{)}^T& =\varphi (t{)}^{\wedge }\\ \dot{R}(t)=\varphi (t{)}^{\wedge }R(t)& =\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]R(t)\end{array}\text{\hspace{1em}(1.7.1)}$$意味着，每对旋转求一次导数，只需左乘一个 ${\varphi }^{\wedge }(t)$ 矩阵即可。
$R(t)$ 在 $t_0=0$($R(0)=I$)附近进行一阶泰勒展开：$$\begin{array}{rl}R(t)& \approx R(t_0)+\dot{R}(t_0)(t-t_0)\\ & =I+\varphi (t_0{)}^{\wedge }t\\ & =I+{\varphi }_0^{\wedge }t\end{array}$$可见，$\varphi$ 反应了一阶导数性质，它位于正切空间（tangent space）上。
在 $t_0$ 附近，假设 $\varphi (t_0)={\varphi }_0$ 保持不变，由式子(1.7.1)可知，求解微分方程，且有初始值 $R(t)=I$，解得$$R(t)=\exp ({\varphi }_0^{\wedge }t)$$说明在 $t=0$ 附近，旋转矩阵由 $\exp ({\varphi }_0^{\wedge }t)$ 计算出来。可见，旋转矩阵之间得乘法可以通过上式得指数映射转换为加法，而 $\varphi$ 正是对应到 $SO(3)$ 上的李代数 $\mathfrak{so}(3)$，因此引出李代数。李代数空间就是有旋转向量组成的空间，其物理意义就是旋转向量。

每个李群都由对应的李代数，李代数描述了李群的局部性质（单位元内的正切空间）。
李代数（Lie Algebra）的定义如下：李代数由一个集合 $\mathbb{V}$、一个数域 $\mathbb{F}$ 和一个二元运算 $[,]$ (李括号)组成，满足一定性质，则可称($\mathbb{V}$, $\mathbb{F}$,$[,]$)为一个李代数，记作 $\mathfrak{g}$。
如 $SO(3)$ 对应的李代数是定义在 ${\mathbb{R}}^3$ 上的向量，记作 $\varphi$。每个 $\varphi$ 都可以生成一个反对称矩阵：$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in {\mathbb{R}}^{3\times 3}$$由于向量 $\varphi$ 与反对称矩阵 $\Phi$ 是一一对应的，因此称 $\mathfrak{so}(3)$ 的元素是三维向量或者三维反对称矩阵。 s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } (1.7.2) \mathfrak{so}(3)=\{\phi\in\mathbb{R}^3,\Phi=\phi^\wedge\in\mathbb{R}^{3\times3}\}\tag{1.7.2} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}(1.7.2)李代数 $\mathfrak{so}(3)$ 与李群 $SO(3)$ 的关系由指数映射给定：$$R=\exp ({\varphi }^{\wedge })\text{\hspace{1em}(1.7.3)}$$
同理，对于 $SE(3)$ 李群，对应的李代数 $\mathfrak{se}(3)$。有$$\mathfrak{se}(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \mathfrak{so}(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}\text{\hspace{1em}(1.7.4)}$$把每个 $\mathfrak{se}(3)$ 元素记作 $\xi$，它是一个六维向量。前三维 $\rho$ 为平移相关的向量（$t=J\rho$，$t$ 为平移向量，$J$ 为雅可比矩阵(系数矩阵)），后三维为上文提到的与旋转的向量（$\mathfrak{so}(3)$ 的元素）。同时拓展了 ${}^{\wedge }$ 符号的含义。在 $\mathfrak{se}(3)$ 中，用该符号将一个六维向量转换成四维矩阵，但是不再表示为反对称。

由式(1.7.3)知，存在指数映射（Exponential Map），如何求解？
任意矩阵的指数映射可以写成一个泰勒展开，但只有在收敛的情况下才会由结果，其结果仍是一个矩阵：$$\exp (A)=\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n$$同样地，对于 $\mathfrak{so}(3)$ 中的元素 $\varphi$ 可得， $$\exp ({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$但此定义无法计算（无穷次幂）。令 $\varphi =\theta a$，类似于旋转向量，$\theta$ 表示大小，$a$ 表示方向（$a$ 为单位向量）。
由式子(1.1.4)且带入上式可得
$$\begin{array}{rl}\exp ({\varphi }^{\wedge })& =\exp (\theta a^{\wedge })\\ & =\sum _{n=0}^{\infty }\frac{1}{n!}(\theta a^{\wedge }{)}^n\\ & =I+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2{a}^{\wedge }{a}^{\wedge }+\frac{1}{3!}{\theta }^3{a}^{\wedge }{a}^{\wedge }{a}^{\wedge }+\frac{1}{4!}{\theta }^4(a^{\wedge }{)}^4+\cdots \\ & =a{a}^T+\underset{\sin \theta }{\underbrace{(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\cdots )}}{a}^{\wedge }-\underset{\cos \theta }{\underbrace{(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\cdots )}}{a}^{\wedge }{a}^{\wedge }\\ & =\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }\end{array}$$
整理出
$$\exp (\theta a^{\wedge })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }\text{\hspace{1em}(1.7.5)}$$

(不将 ${\varphi }^{\wedge }$ 展开为 $\theta a^{\wedge }$ ，则有如下公式)
$$\exp ({\varphi }^{\wedge })=I+\frac{\sin (\Vert \varphi \Vert )}{\Vert \varphi \Vert }{\varphi }^{\wedge }+\frac{1-\cos (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^2}({\varphi }^{\wedge }{)}^2$$

上式与式子(1.4.1)如出一辙。这说明 $\mathfrak{so}(3)$ 就是所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。那么对于求李代数的指数映射和求李群的对数映射分别直接利用式子(1.7.5)和式子(1.4.3)(1.4.4)求得。（对于 $\theta$ 来说，角度应该固定在 $\pm \pi$ 之间，就存在李群和李代数的单射）。旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。
$\mathfrak{se}(3)$ 上的指数映射形式如下：$$\begin{array}{rl}\exp ({\xi }^{\wedge })& =\sum _{n=0}^{\infty }\frac{1}{n!}({\xi }^{\wedge }{)}^n=\sum _{n=0}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^n\\ & =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right]\\ & \stackrel{△}{=}\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]=T.\end{array}\text{\hspace{1em}(1.7.6)}$$其中 $J$ 同式子(1.7.5)可得 $$\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }\stackrel{def}{=}J\text{\hspace{1em}(1.7.7)}$$
$$t=J\rho \text{\hspace{1em}(1.7.8)}$$

---

	公 式
$SO(3)\to \mathfrak{so}(3)$	(1.4.3)(1.4.4)
$\mathfrak{so}(3)\to SO(3)$	(1.7.5)
$SE(3)\to \mathfrak{se}(3)$	(1.4.3)(1.4.4)(1.7.8)
$\mathfrak{se}(3)\to SE(3)$	(1.7.6)(1.7.7)

---

1.8 李代数求导

对于李群 $SO(3)$ 和李代数 $\mathfrak{se}(3)$ 来说，是否有
$$\begin{array}{rl}{R}_1{R}_2& =\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })\\ & \stackrel{?}{=}\exp ({\varphi }_1^{\wedge }+{\varphi }_2^{\wedge })\\ & =\exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })\end{array}$$
由 $$\exp ({\varphi }_1^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }_1^{\wedge }{)}^n=1+{\varphi }_1^{\wedge }+\frac{1}{2!}({\varphi }_1^{\wedge }{)}^2+\cdots +\frac{1}{n!}({\varphi }_1^{\wedge }{)}^n+\cdots$$可得出$$\begin{array}{rl}\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })& =\left[1+{\varphi }_1^{\wedge }+\frac{1}{2!}({\varphi }_1^{\wedge }{)}^2+\cdots +\frac{1}{n!}({\varphi }_1^{\wedge }{)}^n+\cdots \right]\left[1+{\varphi }_2^{\wedge }+\frac{1}{2!}({\varphi }_2^{\wedge }{)}^2+\cdots +\frac{1}{n!}({\varphi }_2^{\wedge }{)}^n+\cdots \right]\\ & =1+\left({\varphi }_1^{\wedge }+{\varphi }_2^{\wedge }\right)+\frac{1}{2!}\left(({\varphi }_2^{\wedge }{)}^2+({\varphi }_2^{\wedge }{)}^2+2{\varphi }_1^{\wedge }{\varphi }_2^{\wedge }\right)+\cdots \end{array}$$而$$\begin{array}{rl}\exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })& =\exp ({\varphi }_1^{\wedge }+{\varphi }_2^{\wedge })\\ & =1+({\varphi }_1^{\wedge }+{\varphi }_2^{\wedge })+\frac{1}{2!}({\varphi }_1^{\wedge }+{\varphi }_2^{\wedge }{)}^2+\cdots \\ & =1+({\varphi }_1^{\wedge }+{\varphi }_2^{\wedge })+\frac{1}{2!}\left(({\varphi }_2^{\wedge }{)}^2+({\varphi }_2^{\wedge }{)}^2+{\varphi }_1^{\wedge }{\varphi }_2^{\wedge }+{\varphi }_2^{\wedge }{\varphi }_1^{\wedge }\right)+\cdots \end{array}$$由于 ${\varphi }^{\wedge }$ 为矩阵，${\varphi }_1^{\wedge }{\varphi }_2^{\wedge }\ne {\varphi }_2^{\wedge }{\varphi }_1^{\wedge }$，该式不成立。
也就是说，当 $A,B$ 为矩阵时，$$\ln (\exp (A)\exp (B))\ne A+B$$由Baker-Campbell-Hausdorff公式（BCH公式）给出展开式前几项$$\ln (\exp (A)\exp (B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\cdots$$其中 $[]$ 为李括号。
特别地，对于 $SO(3)$ 上的李代数 $[\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })){]}^{\vee }$，当 ${\varphi }_1$ 或 ${\varphi }_2$ 为小量时（小量的二次以上的项都可以被忽略）。BCH近似表达为$$[\ln \left(\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })\right){]}^{\vee }\approx \{\begin{array}{l}{J}_l^{-1}({\varphi }_2){\varphi }_1+{\varphi }_2当{\varphi }_1为小量\\ {\varphi }_1+J_r^{-1}({\varphi }_1){\varphi }_2当{\varphi }_2为小量\end{array}\text{\hspace{1em}(1.8.1)}$$其中，左乘BCH近似雅可比 $J_l$ 事实上就是式子(1.7.7)
$$J_l=J=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$
逆为：
$$J_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}I+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)a{a}^T-\frac{\theta }{2}{a}^{\wedge }$$
右乘雅可比 $J_r$ 仅需对自变量去负号即可

$$J_r(\varphi )=J_l(-\varphi )$$ $$J_r(\theta )=J_l(-\theta )$$

(不将 ${\varphi }^{\wedge }$ 展开为 $\theta a^{\wedge }$ ，则有如下公式)
$$J_r({\varphi }^{\wedge })=I-\frac{1-\cos (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^2}{\varphi }^{\wedge }+\left(\frac{\Vert \varphi \Vert -\sin (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^3}\right)({\varphi }^{\wedge }{)}^2$$ $$J_r^{-1}({\varphi }^{\wedge })=I+\frac{1}{2}{\varphi }^{\wedge }+\left(\frac{1}{\Vert \varphi {\Vert }^2}-\frac{1+\cos (\Vert \varphi \Vert )}{2\cdot \Vert \varphi \Vert \cdot \sin (\Vert \varphi \Vert )}\right)({\varphi }^{\wedge }{)}^2$$
表明，当对一个旋转矩阵 $R_2$ (李代数为 ${\varphi }_2$ )左乘一个微小的旋转矩阵 $R_1$（李代数为 ${\varphi }_1$）时，可以近似的看操作，在原有的李代数 ${\varphi }_2$ 的基础上添加了一项 $J_l({\varphi }_2{)}^{-1}{\varphi }_1$。
上述可理解为：对某个旋转 $R$，对应的李代数为 $\varphi$。左乘一个微小的旋转 $\Delta R$，对应的李代数为 $\Delta \varphi$。那么在李群上的结果就是 $\Delta R\cdot R$，而在李代数上，根据BCH近似，为 $J_l^{-1}(\varphi )\Delta \varphi +\varphi$，写成$$\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp \left((J_l^{-1}(\varphi )\Delta \varphi +\varphi {)}^{\wedge }\right)\text{\hspace{1em}(1.8.2)}$$反之，如果在李代数上进行加法 $\Delta \varphi +\varphi$，那么可近似为李群上代左右雅可比的乘法：$$\exp \left((\varphi +\Delta \varphi {)}^{\wedge }\right)=\exp \left(J_l(\varphi )\Delta {\varphi }^{\wedge }\right)\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp \left(J_r(\varphi )\Delta {\varphi }^{\wedge }\right)\text{\hspace{1em}(1.8.3)}$$（上式中的雅可比矩阵为系数矩阵，左乘右乘体现在李群上）
同理，对于李群 $SE(3)$ 来说，也有类似的BCH近似：$$\begin{gathered} \exp (\Delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\approx \exp \left(({\mathcal{J}}_l^{-1}\Delta \xi +\xi {)}^{\wedge }\right) \\ \exp ({\xi }^{\wedge })\exp (\Delta {\xi }^{\wedge })\approx \exp \left(({\mathcal{J}}_r^{-1}\Delta \xi +\xi {)}^{\wedge }\right)\text{\hspace{1em}(1.8.4)} \end{gathered}$$其中，${\mathcal{J}}_l$ 形式比较复杂，是一个 $6\times 6$ 矩阵。（过于繁琐，不使用。）

对相机的位姿估计，该位姿是由 $SO(3)$上的旋转矩阵或 $SE(3)$ 上的变换矩阵描述的。设某一时刻相机的位姿为 $T$，相机观察到可一个世界坐标位于 $p$ 的点，产生了一个观测数据 $z$，有$$z=Tp+w$$其中 $w$ 为噪声。因此会定义理想观测和实际数据的误差为：$$e=z-Tp$$假设共有 $N$ 个路标点 $p$ 和观测点 $z$，那么对机器人的位姿估计就相当于寻找一个最优的 $T$，使整体误差最小化：$$\underset{T}{\min }J(T)=\underset{T}{\min }\left(\sum _{i=1}^N\Vert z_i-T{p}_i{\Vert }_2^2\right)\text{\hspace{1em}(1.8.5)}$$显然，求解此问题需要计算目标函数 $J(T)$ 关于变换矩阵 $T$ 的导数。

通常来讲，我们会构建与位姿有关的函数，然后去讨论该函数对于位姿的导数，已调整当前的估计值。求导分为两种思路：导数模型，对 $R$ 对应的李代数 $\varphi$ 加上小量 $\delta \varphi$，求相当于小量 $\delta \varphi$ 的变化率（通过导数的定义）；扰动模型，对 $R$ 左乘或右乘一个小量 $\Delta R$，求相对于小量 $\Delta R$ 的李代数 $\phi$ 的变化率（相对于扰动的变化率）。

导数模型：$$\begin{array}{rl}\frac{\partial \exp ({\varphi }^{\wedge })p}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{\exp (\varphi +\delta \varphi {)}^{\wedge }p-\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp (J_l^{-1}(\varphi )\delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{(I+J_l^{-1}(\varphi )\delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{(J_l^{-1}(\varphi )\delta \varphi {)}^{\wedge }\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{-(\exp ({\varphi }^{\wedge })p{)}^{\wedge }(J_l^{-1}(\varphi )\delta \varphi )}{\delta \varphi }\\ & =-(Rp{)}^{\wedge }{J}_l\end{array}\text{\hspace{1em}(1.8.6)}$$

扰动模型：$$\begin{array}{rl}\frac{\partial Rp}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-(\exp ({\varphi }^{\wedge })p{)}^{\wedge }\phi }{\phi }\\ & =-(Rp{)}^{\wedge }\end{array}\text{\hspace{1em}(1.8.7)}$$由此可知，扰动模型更为实用。

给出 $SE(3)$ 上的扰动模型：某一空间点 $p$ 经过一次变换 $T$ （李代数为 $\xi$）之后，得到 $Tp$。给出 $T$ 左乘一个扰动 $\Delta T=\exp (\delta {\xi }^{\wedge })$，设扰动项的李代数为 $\delta \xi =[\delta \rho \delta \varphi {]}^T$，得出：$$\begin{array}{rl}\frac{\partial (Tp)}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{\lim }\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{(I+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })p}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}Rp+t\\ 1\end{array}\right]}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t)+\delta \rho \\ 0\end{array}\right]}{[\delta \rho \delta \varphi {]}^T}分子布局\\ & =\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\stackrel{def}{=}(Tp{)}^{\odot }\end{array}\text{\hspace{1em}(1.8.8)}$$

---

估计轨迹误差
估计轨迹 $T_{esti,i}$ 和真实轨迹 $T_{gt,i}$，其中 $i=1,\cdots ,N$，则有绝对轨迹误差(Absolute Trajectory Error,ATE)$$AT{E}_{all}=\sqrt{\frac{1}{N}\sum _{i=1}^N\Vert \log (T_{gt,i}^{-1}{T}_{esti,i}{)}^{\vee }{\Vert }_2^2}$$实际上是每个位姿李代数的均方根误差(Root-Mean-Squared-Error,RMSE)。

---

齐次坐标

二维平面上，一条直线可以写作$$l:ax+by+c=0$$ 表示直线 $l$ 的向量可写为$$l=(a,b,c{)}^T$$直线上一点 $p=(x,y)$，写作齐次坐标为 $\tilde{p}=(x,y,1)$，则直线 $l$ 可用向量内积来表示
$$ax+by+c=(a,b,c)(x,y,1)=l^T\cdot \tilde{p}={\tilde{p}}^T\cdot l=0$$
同理三维空间，有平面 $\pi$：
$$ax+by+cz+d=(a,b,c,d)(x,y,z,1)={\pi }^T\cdot \tilde{P}={\tilde{P}}^T\cdot \pi =0$$

在齐次坐标下，可以用 $p$、$q$ 两个点的齐次坐标叉乘结果来表示一条直线 $l$，即 $l=\tilde{p}\times \tilde{q}$。
证： 记 $l=\tilde{p}\times \tilde{q}$，则有 $${\tilde{p}}^T\cdot (\tilde{p}\times \tilde{q})=0={\tilde{p}}^T\cdot l=l^T\cdot \tilde{p}$$
同理，则有 $${\tilde{q}}^T\cdot (\tilde{p}\times \tilde{q})=0={\tilde{q}}^T\cdot l=l^T\cdot \tilde{q}$$则点 $p$ 和 $q$ 都在直线 $l$ 上。

也可以用两条直线 $l$、 $m$ 的叉乘表示两条直线的交点的齐次坐标 $p$，即 $\tilde{p}=l\times m$
证：两条非异面直线构成一个平面，直线叉乘可得到该平面法向量。两条直线分别为 $l$ 和 $m$，记$$l\times m=\tilde{p}$$则有$$l^T\cdot (l\times m)=0=l^T\cdot \tilde{p}$$
同理$$m^T\cdot (l\times m)=0=m^T\cdot \tilde{p}$$
$p$ 既在直线 $l$ 上，又在直线 $m$ 上，因此点 $p$ 为两直线交点。

齐次坐标下点和向量的区别在于第四维。点 $p$ 的齐次坐标为 $(a,b,c,1)$，向量 $p$ 的齐次坐标为 $(a,b,c,0)$。 向量只有方向，大小，没有起点终点。向量为两个点的差。
如果一个点的齐次坐标最后为0，则认为此点为无限远。