一、数学基础

对于两个非零向量：

a=(x1,y1,z1)T,b=(x2,y2,z2)Ta = ({x_1},{y_1},{z_1})^\mathsf{T},b = ({x_2},{y_2},{z_2})^\mathsf{T}\\ a=(x1,y1,z1)T,b=(x2,y2,z2)T

则向量的内积：
⟨b,a⟩=aTb=x1x2+y1y2+z1z2\left\langle {b,a} \right\rangle = a^\mathsf{T} b = {x_1}{x_2} + {y_1}{y_2} + {z_1}{z_2}\\ ⟨b,a⟩=aTb=x1x2+y1y2+z1z2
其结果为一个标量；

向量的外积：
a×b=∣ijkx1y1z1x2y2z2∣=(y1z2−y2z1,z1x2−z2x1,x1y2−x2y1)T=[0−z1y1z10−x1−y1x10]b\begin{aligned} a \times b &= \left| {\begin{matrix} i&j&k\\ {{x_1}}&{{y_1}}&{{z_1}}\\ {{x_2}}&{{y_2}}&{{z_2}} \end{matrix}} \right| \\ &= ({y_1}{z_2} - {y_2}{z_1},{z_1}{x_2} - {z_2}{x_1},{x_1}{y_2} - {x_2}{y_1})^\mathsf{T}\\ &= \left[ {\begin{matrix} 0&{ - {z_1}}&{{y_1}}\\ {{z_1}}&0&{ - {x_1}}\\ { - {y_1}}&{{x_1}}&0 \end{matrix}} \right] b \end{aligned} a×b=∣∣∣∣∣∣ix1x2jy1y2kz1z2∣∣∣∣∣∣=(y1z2−y2z1,z1x2−z2x1,x1y2−x2y1)T=⎣⎡0z1−y1−z10x1y1−x10⎦⎤b
向量的外积依旧是个向量，其方向为用右手从a转向b，其模长为：
∣a∣∣b∣sin⁡⟨a,b⟩\left| a \right|\left| b \right|\sin \left\langle {a,b} \right\rangle ∣a∣∣b∣sin⟨a,b⟩
令矩阵：
a∧=[0−z1y1z10−x1−y1x10]a ^\wedge =\left[ {\begin{matrix} 0&{ - {z_1}}&{{y_1}}\\ {{z_1}}&0&{ - {x_1}}\\ { - {y_1}}&{{x_1}}&0 \end{matrix}} \right] a∧=⎣⎡0z1−y1−z10x1y1−x10⎦⎤
我们称其为a向量的反对称矩阵。

二、旋转矩阵求导

三维的旋转矩阵R构成了一种特殊的正交群SO(3)，变换矩阵T则构成了SE(3):
SO(3)={R∈R3×3∣RRT=I,det⁡(R)=1}SE(3)={T=[Rt0T1]∈R4×4∣R∈R3×3,t∈R3}\begin{aligned} SO(3) &= \{ R \in {\mathbb{R}^{3 \times 3}}|R{R^T} = I,\det (R) = 1\} \\ SE(3) &= \{ T = \left[ {\begin{matrix} R&t \\ {{0^T}}&1 \end{matrix}} \right] \in {\mathbb{R}^{4 \times 4}}|R \in {\mathbb{R}^{3 \times 3}},t \in {\mathbb{R}^3}\} \end{aligned} SO(3)SE(3)={R∈R3×3∣RRT=I,det(R)=1}={T=[R0Tt1]∈R4×4∣R∈R3×3,t∈R3}
需要注意的是，它们对加法不封闭：
R1+R2∉SO(3),T1+T2∉SE(3){R_1} + {R_2} \notin SO(3),{T_1} + {T_2} \notin SE(3) R1+R2∈/SO(3),T1+T2∈/SE(3)

我们首先让旋转矩阵对时间t求导（注意这里是对时间t求导，和后面的左扰动求导参数不同）

因为：
RRT=IRR^T=I RRT=I
若旋转矩阵随时间变化,则：
R(t)R(t)T=IR(t)R(t)^T=I R(t)R(t)T=I
我们设初始时刻姿态为P0,t时刻姿态为Pt，则：
Pt=R(t)P0{P_t} = R(t){P_0} Pt=R(t)P0
两边同时求导得：
dPtdt=dR(t)dtP0\frac{\mathrm{d} P_t }{\mathrm{d} t} = \frac{\mathrm{d} R(t) }{\mathrm{d} t} P_0 dtdPt=dtdR(t)P0
由理论力学的知识得：
V(t)=ω×P(t)=ω×R(t)⋅P0=ω∧R(t)⋅P0V(t) = \omega \times P(t)=\omega \times R(t)\cdot P_0=\omega^\wedge R(t)\cdot P_0 V(t)=ω×P(t)=ω×R(t)⋅P0=ω∧R(t)⋅P0
位置对t求导即速度：
dPtdt=V(t)\frac{\mathrm{d} P_t }{\mathrm{d} t} = V(t) dtdPt=V(t)
所以由上式得
ω∧R(t)⋅P0=dR(t)dtP0\omega^\wedge R(t)\cdot P_0=\frac{\mathrm{d} R(t) }{\mathrm{d} t} P_0 ω∧R(t)⋅P0=dtdR(t)P0
所以有：
dR(t)dt=ω∧R(t)⋅\frac{\mathrm{d} R(t) }{\mathrm{d} t}= \omega^\wedge R(t)\cdot dtdR(t)=ω∧R(t)⋅
又假设t0=0时的初始条件为:
R(t0)=IR(t_0)=I R(t0)=I
所以由矩阵论的知识可得：
R(t)=exp(t⋅ω∧)R(t) = exp(t\cdot \omega^\wedge) R(t)=exp(t⋅ω∧)
又因为：
ϕ=ω⋅t\phi = \omega\cdot t ϕ=ω⋅t
所以最后：
R(t)=exp(ϕ∧)R(t)=exp(\phi^\wedge) R(t)=exp(ϕ∧)
由因为此式对任意时刻都适用，即：
R=exp(ϕ∧)R=exp(\phi^\wedge) R=exp(ϕ∧)

三、李代数求导

3.1 对李代数求导的意义：

假设一个点的世界坐标p已知，又已知在一个位姿下T（位姿T未知）观测到该点的坐标为z，求出最优的T使得||z-Tp||最小，这样的T就是最优估计；

或者也可以这么解释，如果我们得到一组点的k时刻观测数据Z ，上一个时刻k-1对同一组点观测的数据为P（k-1时刻的位姿与世界坐标重合），此时我们估计它从k-1到k的变化矩阵T，这时候存在着一个误差：
e=z−Tpe = z-Tp e=z−Tp
这时候我们需要让这个误差最小，才能让我们估计的T成为最优估计;
所以可以转化为，对于目标函数J(T),使得J(T)最小：
min⁡TJ(T)=∑i=1N∥zi−Tpi∥22\mathop {\min }\limits_T J(T) = \sum\limits_{i = 1}^N {\left\| {{z_i} - T{p_i}} \right\|_2^2} TminJ(T)=i=1∑N∥zi−Tpi∥22
我们需要对Tp求导，通过导数来判断极小值点，求出最优解；
但由于李群的不可加性，所以只能通过李群对应的李代数来进行求导；例如：
∂(Tp)∂T=lim⁡ΔT→0(T+ΔT)p−TpΔT\frac{{\partial (Tp)}}{{\partial T }} =\mathop {\lim }\limits_{\Delta T \to 0} \frac{{(T+\Delta T)p - Tp}}{\Delta T } ∂T∂(Tp)=ΔT→0limΔT(T+ΔT)p−Tp
这个式子是不正确的，因为T+△T不再是SE(3)群，所以要考虑利用其李代数求导；
考虑旋转，平移六个自由度，六个参数：
T=T(ϕ,t),ϕ∈R3,t∈R3T=T(\phi,t),\phi \in \mathbb{R}^3,t \in \mathbb{R}^3 T=T(ϕ,t),ϕ∈R3,t∈R3
这个问题就转变为了对下面的式子：
min⁡ϕ,tJ(ϕ,t)=∑i=1N∥zi−T(ϕ,t)pi∥22\mathop {\min }\limits_{\phi,t} J(\phi,t)=\sum\limits_{i = 1}^N {\left\| {{z_i} - T(\phi,t){p_i}} \right\|_2^2} ϕ,tminJ(ϕ,t)=i=1∑N∥zi−T(ϕ,t)pi∥22
由定义的李代数：
se(3)={ξ=[ρϕ]∈R6∣ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]}se(3) = \{ \xi = \left[ {\begin{matrix} \rho \\ \phi \end{matrix}} \right] \in {\mathbb{R}^6}|\rho \in {\mathbb{R}^3},\phi \in so(3),{\xi ^ \wedge } = \left[ {\begin{matrix} {{\phi ^ \wedge }}&\rho \\ {{0^T}}&0 \end{matrix}} \right]\} se(3)={ξ=[ρϕ]∈R6∣ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]}
最后变为：
min⁡ϕ,ρJ(ϕ,ρ)=∑i=1N∥zi−exp⁡(ϕ∧)pi∥22\mathop {\min }\limits_{\phi,\rho} J(\phi,\rho)=\sum\limits_{i = 1}^N {\left\| {{z_i} - \exp ({\phi ^ \wedge) }{p_i}} \right\|_2^2} ϕ,ρminJ(ϕ,ρ)=i=1∑N∥zi−exp(ϕ∧)pi∥22

3.2 在李群SO(3)上的扰动模型：

对旋转矩阵R进行扰动△R，对应在李代数上为δψ，利用李代数求扰动模型：
∂(Rp)∂φ=lim⁡φ→0exp⁡(φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=−(Rp)∧\begin{aligned} \frac{{\partial (Rp)}}{{\partial \varphi }} &= \mathop {\lim }\limits_{\varphi \to 0} \frac{{\exp ({\varphi ^ \wedge })\exp ({\phi ^ \wedge })p - \exp ({\phi ^ \wedge })p}}{\varphi }\\ &= - {(Rp)^ \wedge } \end{aligned} ∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p=−(Rp)∧
不再叙述推导的过程；

3.3 在李群SE(3)上的扰动模型：

首先说明对应的李代数se(3):
se(3)={ξ=[ρϕ]∈R6∣ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]}se(3) = \{ \xi = \left[ {\begin{matrix} \rho \\ \phi \end{matrix}} \right] \in {\mathbb{R}^6}|\rho \in {\mathbb{R}^3},\phi \in so(3),{\xi ^ \wedge } = \left[ {\begin{matrix} {{\phi ^ \wedge }}&\rho \\ {{0^T}}&0 \end{matrix}} \right]\} se(3)={ξ=[ρϕ]∈R6∣ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]}
其中满足：
exp(ξ∧)=T=[RJρ0T1]exp(\xi ^ \wedge)=T=\left[ {\begin{matrix} R&J\rho \\ {{0^T}}&1 \end{matrix}} \right] exp(ξ∧)=T=[R0TJρ1]
所以：
∂(Tp)∂δξ=∂(exp⁡(ϕ∧))∂δξ=lim⁡δξ→0exp⁡(δξ∧)exp⁡(ξ∧)p−exp⁡(ξ∧)pδξ=[I3×3−(Rp+t)∧01×3T01×3T]\begin{aligned} \frac{{\partial (Tp)}}{{\partial \delta \xi }} &=\frac{{\partial (\exp ({\phi ^ \wedge) })}}{{\partial \delta \xi }}\\ &=\mathop {\lim }\limits_{\delta \xi \to 0} \frac{{\exp ({\delta \xi ^ \wedge })\exp ({\xi ^ \wedge })p - \exp ({\xi ^ \wedge })p}}{\delta \xi }\\ &= \left[ {\begin{matrix} {{I_{3 \times 3}}}&{ - {{(Rp + t)}^ \wedge }} \\ {0_{1 \times 3}^T}&{0_{1 \times 3}^T} \end{matrix}} \right] \end{aligned} ∂δξ∂(Tp)=∂δξ∂(exp(ϕ∧))=δξ→0limδξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p=[I3×301×3T−(Rp+t)∧01×3T]

总结

李群李代数的左扰动模型是非常有用的，尤其是在估计位姿的时候。
对于左扰动，我们定义的变换矩阵为：
Tk,k−1T_{k,k-1} Tk,k−1
(这个矩阵的物理意义为：上一个观测位姿下观测点的坐标左乘变换矩阵T可以得到下一个观测位姿下的观测点坐标)
Pk=Tk,k−1⋅Pk−1P_k =T_{k,k-1}\cdot P_{k-1} Pk=Tk,k−1⋅Pk−1

左扰动定义为：
δT=δTk,k−1\delta T=\delta T_{k,k-1} δT=δTk,k−1
所以有：

Tk,k−1=δTk,k−1⋅Tk−1T_{k,k-1} = \delta T_{k,k-1} \cdot T_{k-1} Tk,k−1=δTk,k−1⋅Tk−1
同理对于右扰动模型：

Tk−1⋅δTk−1,k=Tk−1,kT_{k-1} \cdot \delta T_{k-1,k} =T_{k-1,k}\\ Tk−1⋅δTk−1,k=Tk−1,k

SLAM--李群李代数左扰动模型相关推荐

【李代数求偏导】SLAM中李代数SE3求偏导左扰动模型和右扰动模型的区别
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言结论 1.左扰动模型: 2.右扰动模型: 2.1右扰动+右乘更新 2.2右扰动+左乘更新 3.将T展开后求偏导(具体看 ...
SE(3)和se(3)，左扰动模型
1. SE(3)和se(3) 李群 S E ( 3 ) = { T = Δ [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ R 3 × 3 , R R ⊤ = I , det ⁡ ( R ) ...
李群与李代数2：李代数求导和李群扰动模型
李群与李代数2:李代数求导和李群扰动模型 1. 整体误差最小化引出求导问题 2. BCH公式与近似形式 2.1 BCH公式 2.2 BCH线性近似 2.3 BCH近似的意义 3. 微分模型--李代数求 ...
SLAM学习--------相机位姿表示-李群李代数
slam 求解相机的位姿求解核心思想:将有约束的李群问题转换成无约束的李代数问题,然后使用高斯牛顿算法或者LM(列文伯格-马夸尔特法)求解. 人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投 ...
SLAM④----李群与李代数
目录 4.0 本章任务 4.1 李群和李代数基础 4.1.1 群 4.1.2 李代数的引出 4.1.3李代数的定义 4.1.4 李代数 so(3) 4.1.5 李代数 se(3) 4.2 指数与对数映 ...
SLAM中相机位姿求解（李群李代数）
前言 slam中一个关键问题之一就是求解相机的位姿,人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投影误差,3D几何误差等等,希望使得误差最小,进而求得比较准确的相机位姿.举一个重投影例子: ...
SLAM本质剖析番外-李群李代数的微分和导数
0. 简介这几个月,博主已经从SLAM算法的使用向着算法的数学推导进行了记录和分享,之前也分享了李群李代数关注核心一文,从现象中解释了李群和李代数表达的含义.但是这还不够,所以这次作者作为SLAM本 ...
李代数求导与扰动模型
我们经常构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值.表示姿态的李群与表示位姿的李群上并没有定义加法,而求导数是需要加法的.因此此时如何求导就成了一个需要解决的问题,它们的李 ...
李群李代数：李代数求导
雅可比雅可比分为左乘雅可比和右乘雅可比,分别记为JlJ_lJl和JrJ_rJr.左乘雅可比即为李代数se(3)中的JJJ Jl=J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa ...
视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...

SLAM--李群李代数左扰动模型

目录