slam框架

1
2
3
4
5
6
graph LR
A[传感器信息读取]-->B[前端视觉里程计]
B-->C["后端（非线性优化）"]
C-->D[建图]
A-->E[回环检测]
E-->C

三维空间刚体运动

旋转矩阵

点、向量、坐标系

任意向量在指定的基(e1,e2,e3)(\mathbf{e_1},\mathbf{e_2},\mathbf{e_3})下有一坐标

a=[e1,e2,e3][a1a2a3]=a1e1+a2e2+a3e3\mathbf{a}= \begin{bmatrix} \mathbf{e_1},\mathbf{e_2},\mathbf{e_3} \end{bmatrix} \begin{bmatrix} a_1\\a_2\\a_3 \end{bmatrix} =a_1\mathbf{e_1}+a_2\mathbf{e_2}+a_3\mathbf{e_3}

(a1,a2,a3)T(a_1,a_2,a_3)^T称为a\mathbf{a}在此基下的坐标。

对于a,b∈R3\mathbf{a},\mathbf{b}\in \mathbb{R}^3，内积为

a⋅b=aTb=∑i=13aibi=∣a∣∣b∣cos⁡⟨a,b⟩\mathbf{a}\cdot\mathbf{b}=\mathbf{a}^T\mathbf{b}=\sum^3_{i=1}a_ib_i=|\mathbf{a}||\mathbf{b}|\cos\lang\mathbf{a},\mathbf{b}\rang

外积

a×b=[e1e2e2a1a2a3b1b2b3]=[a2b3−a3b2a3b1−a1b3a1b2−a2b1]=[0−a3a2a30−a1−a2a10]b≜a∧b\mathbf{a}\times\mathbf{b}= \begin{bmatrix} \mathbf{e_1}&\mathbf{e_2}&\mathbf{e_2}\\ a_1&a_2&a_3\\ b_1&b_2&b_3\\ \end{bmatrix} =\begin{bmatrix} a_2b_3-a_3b_2\\ a_3b_1-a_1b_3\\ a_1b_2-a_2b_1\\ \end{bmatrix} =\begin{bmatrix} 0&-a_3&a_2\\ a_3&0&-a_1\\ -a_2&a_1&0\\ \end{bmatrix}\mathbf{b} \triangleq \mathbf{a}\land\mathbf{b}

a∧=[0−a3a2a30−a1−a2a10]\mathbf{a}\land= \begin{bmatrix} 0&-a_3&a_2\\ a_3&0&-a_1\\ -a_2&a_1&0\\ \end{bmatrix}

a∧\mathbf{a}\land是一个反对称函数。

坐标间的欧氏变换

某单位正交基(e1,e2,e3)(\mathbf{e_1},\mathbf{e_2},\mathbf{e_3})经过旋转得到(e1′,e2′,e3′)(\mathbf{e_1'},\mathbf{e_2'},\mathbf{e_3'})，对于一个向量a\mathbf{a}，在两个坐标系下的坐标为[a1,a2,a3]T[a_1,a_2,a_3]^T和[a1′,a2′,a3′][a_1',a_2',a_3']，那么有

[e1e2e3][a1a2a3]=[e1′e2′e3′][a1′a2′a3′]\begin{bmatrix} \mathbf{e_1}&\mathbf{e_2}&\mathbf{e_3} \end{bmatrix} \begin{bmatrix} a_1\\a_2\\a_3 \end{bmatrix} = \begin{bmatrix} \mathbf{e_1'}&\mathbf{e_2'}&\mathbf{e_3'} \end{bmatrix} \begin{bmatrix} a_1'\\a_2'\\a_3' \end{bmatrix}

对等式左右同时左乘[e1Te2Te3T]\begin{bmatrix} \mathbf{e_1^T}\\\mathbf{e_2^T}\\\mathbf{e_3^T} \end{bmatrix}，得到

[a1a2a3]=[e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′][a1′a2′a3′]≜Ra′\begin{bmatrix} a_1\\a_2\\a_3\\ \end{bmatrix} = \begin{bmatrix} \mathbf{e_1^Te_1'}&\mathbf{e_1^Te_2'}&\mathbf{e_1^Te_3'}\\ \mathbf{e_2^Te_1'}&\mathbf{e_2^Te_2'}&\mathbf{e_2^Te_3'}\\ \mathbf{e_3^Te_1'}&\mathbf{e_3^Te_2'}&\mathbf{e_3^Te_3'}\\ \end{bmatrix} \begin{bmatrix} a_1'\\a_2'\\a_3' \end{bmatrix} \triangleq \mathbf{Ra'}

R\mathbf{R}称为旋转矩阵。

R\mathbf{R}是行列式为1的正交矩阵。

n维旋转矩阵构成特殊正交群SO(n)SO(n)：

SO(n)={R∈Rn×n∣RRT=I,det⁡(R)=1}.SO(n)=\{\mathbf{R}\in\mathbb{R}^{n\times n}|\mathbf{RR^T=I},\det(\mathbf{R})=1\}.

RT\mathbf{R^T}刻画了一个相反的旋转

a′=R−1a=RTa\mathbf{a'}=\mathbf{R^{-1}a}=\mathbf{R^Ta}

把旋转和平移合到一起，得到

a′=Ra+t\mathbf{a'}=\mathbf{Ra}+\mathbf{t}

变换矩阵与齐次坐标

重写上式，得到

[a′1]=[Rt0T1][a1]≜T[a1]\begin{bmatrix} \mathbf{a'}\\ 1\\ \end{bmatrix} = \begin{bmatrix} \mathbf{R}&\mathbf{t}\\ \mathbf{0^T}&1\\ \end{bmatrix} \begin{bmatrix} \mathbf{a}\\ 1\\ \end{bmatrix} \triangleq \mathbf{T} \begin{bmatrix} \mathbf{a}\\ 1\\ \end{bmatrix}

在三维坐标末尾添1，得到齐次坐标。称矩阵T\mathbf{T}为变换矩阵

依靠齐次坐标和变换矩阵，两次变换叠加可以写成

b~=T1a~,c~=T2b~⇒c~=T1T2a~\tilde{\mathbf{b}}=\mathbf{T_1\tilde{a}},\tilde{\mathbf{c}}=\mathbf{T_2\tilde{b}} \Rightarrow \tilde{\mathbf{c}}=\mathbf{T_1T_2\tilde{a}}

变换矩阵构成特殊欧氏群$$SE(n)$$：

SE(n)={T=[Rt0T1]∈R(n+1)×(n+1)∣R∈SO(3),t∈Rn×n}SE(n)=\{\mathbf{T}= \begin{bmatrix} \mathbf{R}&\mathbf{t}\\ \mathbf{0^T}&1\\ \end{bmatrix} \in\mathbb{R}^{(n+1)\times(n+1)}| \mathbf{R}\in SO(3), \mathbf{t}\in\mathbb{R}^{n\times n} \}

T−1=[RT−RTt0T1]\mathbf{T^{-1}}= \begin{bmatrix} \mathbf{R^T}&-\mathbf{R^Tt}\\ \mathbf{0^T}&1 \end{bmatrix}

旋转向量和欧拉角

旋转向量

任意旋转都可以用一个旋转轴n\mathbf{n}和一个旋转角θ\theta来刻画。我们可以使用一个向量，其方向与旋转轴一致，长度对于旋转角，这种向量称为旋转向量（轴角/角轴）。

罗德里格斯公式

R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θtr(n∧)\mathbf{R}=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{nn^T}+\sin\theta tr(\mathbf{n\land})

两边取迹，得到

tr(R)=cos⁡θtr(I)+(1−cos⁡θ)tr(nnT)+sin⁡θtr(n∧)=3cos⁡θ+(1−cos⁡θ)=1+2cos⁡θ\begin{aligned} tr(\mathbf{R})&=\cos\theta tr(\mathbf{I})+(1-\cos\theta)tr(\mathbf{nn^T})+\sin\theta tr(\mathbf{n\land})\\ &=3\cos\theta+(1-\cos\theta)\\ &=1+2\cos\theta\\ \end{aligned}

由此

θ=arccos⁡tr(R)−12\theta = \arccos\frac{tr(\mathbf{R})-1}{2}

旋转轴上的向量在旋转后不发生改变：

Rn=n\mathbf{Rn}=\mathbf{n}

由此，转轴n\mathbf{n}是矩阵R\mathbf{R}的特征值1对应的特征向量。求解此方程归一化后就可以得到旋转轴。

这样，我们就得到了旋转矩阵与旋转向量的转换关系。

欧拉角

欧拉角使用三个分离的转角，把一个旋转分解成3次绕不同轴的旋转。

常用的一种欧拉角是使用“偏航-俯仰-滚转”（yaw-pitch-roll）3个转角描述旋转，等价于ZYX轴的旋转。对任意旋转分解如下：

  1. 绕物体的Z轴旋转，得到偏航角yaw。
  2. 绕旋转之后的Y轴旋转，得到俯仰角pitch。
  3. 绕旋转之后的X轴旋转，得到滚转角roll。

此时，任意旋转可以使用[r,p,y]T[r,p,y]^T三维向量描述。

欧拉角的重大缺点是会遇到万向锁问题：在俯仰角为±90∘\pm 90^{\circ}时，第一次旋转与第三次旋转共用一个轴使得系统丢失一个自由度。这称为奇异性问题。

四元数

四元数是紧凑的，没有奇异性。

q=q0+q1i+q2j+q3k\mathbf{q}=q_0+q_1i+q_2j+q_3k

i,j,ki,j,k是四元数的三个虚部。

{i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j\left\{ \begin{matrix} i^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\\ \end{matrix} \right.

有时，也用一个标量和一个向量表示四元数：

q=[s,v]T, s=q0∈R, v=[q1,q2,q3]T∈R3\mathbf{q}=[s,v]^T,\,s=q_0\in\mathbb{R},\,\mathbf{v}=[q_1,q_2,q_3]^T\in\mathbb{R^3}

ss称为四元数实部，而v\mathbf{v}称为它的虚部，如果虚部为0\mathbf{0}，称为实四元数；反之，实部为0，称为虚四元数。

四元数运算

有两个四元数qa=[sa,va]T,qb=[sb,vb]T\mathbf{q_a}=[s_a,\mathbf{v_a}]^T,\mathbf{q_b}=[s_b,\mathbf{v_b}]^T

  1. 加法和减法

    qa±qb=[sa±sb,va±vb]T\mathbf{q_a}\pm\mathbf{q_b}=[s_a\pm s_b,\mathbf{v_a}\pm\mathbf{v_b}]^T

  2. 乘法

    qaqb=[sasb−vaTvb,savb+sbva+va×vb]T\mathbf{q_a}\mathbf{q_b}=[s_as_b-\mathbf{v_a}^T\mathbf{v_b},s_a\mathbf{v_b}+s_b\mathbf{v_a}+\mathbf{v_a}\times\mathbf{v_b}]^T

    四元数乘法通常是不可交换的，除非va,vb\mathbf{v_a},\mathbf{v_b}共线。

  3. 模长

    ∣qa∣=sa2+vaTva|\mathbf{q_a}|=\sqrt{s_a^2+\mathbf{v_a}^T\mathbf{v_a}}

    ∣qaqb∣=∣qa∣∣qb∣|\mathbf{q_a}\mathbf{q_b}|=|\mathbf{q_a}||\mathbf{q_b}|

  4. 共轭

    qa∗=[sa,−va]T\mathbf{q_a^*}=[s_a,-\mathbf{v_a}]^T

    q∗q=qq∗=[s2+vTv,0]T=[∣qa∣2,0]T\mathbf{q^*q}=\mathbf{qq^*}=[s^2+\mathbf{v}^T\mathbf{v},\mathbf{0}]^T=[|\mathbf{q_a}|^2,\mathbf{0}]^T

  5. 逆

    q−1=q∗∣q∣2\mathbf{q^{-1}}=\frac{\mathbf{q^*}}{|\mathbf{q}|^2}

    qq−1=q−1q=1\mathbf{q}\mathbf{q}^{-1}=\mathbf{q}^{-1}\mathbf{q}=\mathbf{1}

    (qaqb)−1=qa−1qb−1(\mathbf{q_a}\mathbf{q_b})^{-1}=\mathbf{q_a}^{-1}\mathbf{q_b}^{-1}

  6. 数乘

    kq=[ks,kv]Tk\mathbf{q}=[ks,k\mathbf{v}]^T

四元数表示旋转

空间中一三维点p=[x,y,z]∈R3\mathbf{p}=[x,y,z]\in\mathbb{R}^3，以及单位四元数q\mathbf{q}指定的旋转。p\mathbf{p}经过旋转之后变为p′\mathbf{p}'。使用矩阵表述，有p′=Rp\mathbf{p'}=\mathbf{Rp}。

使用四元数，如下

p=[0,x,y,z]T=[0,v]T\mathbf{p}=[0,x,y,z]^T=[0,\mathbf{v}]^T

相当于把四元数的3个虚部与空间中三个轴对应。那么，旋转后的p′\mathbf{p'}可表示为：

p′=qpq−1\mathbf{p'}=\mathbf{q}\mathbf{p}\mathbf{q}^{-1}

四元数到其他旋转表示的变换

设q=[s,v]T\mathbf{q}=[s,\mathbf{v}]^T，定义如下记号

q+=[s−vTvsI+v∧],q⊕=[s−vTvsI−v∧]\mathbf{q}^+=\begin{bmatrix} s&-\mathbf{v}^T\\ \mathbf{v}&s\mathbf{I}+\mathbf{v}^\land \end{bmatrix},\quad \mathbf{q}^\oplus=\begin{bmatrix} s&-\mathbf{v}^T\\ \mathbf{v}&s\mathbf{I}-\mathbf{v}^\land \end{bmatrix}

于是，可以把四元数乘法写成矩阵形式

q1+q2=[s−v1Tv1sI+v1∧][s2v2]=[s1s2−v1Tv2s1v2+s2v1+v1∧v2]=q1q2\mathbf{q_1^+}\mathbf{q_2}=\begin{bmatrix} s&-\mathbf{v_1}^T\\ \mathbf{v_1}&s\mathbf{I}+\mathbf{v_1}^\land \end{bmatrix} \begin{bmatrix} s_2\\ \mathbf{v_2} \end{bmatrix} = \begin{bmatrix} s_1s_2-\mathbf{v_1}^T\mathbf{v_2}\\ s_1\mathbf{v_2}+s_2\mathbf{v_1}+\mathbf{v_1}^\land\mathbf{v_2}\\ \end{bmatrix} =\mathbf{q_1}\mathbf{q_2}

同理，有

q1q2=q1+q2=q2⊕q1\mathbf{q_1q_2}=\mathbf{q_1^+}\mathbf{q_2}=\mathbf{q_2^\oplus}\mathbf{q_1}

根据前面说法，有

p′=qpq−1=q+p+q−1=q+(q−1)⊕p\mathbf{p'}=\mathbf{qpq}^{-1}=\mathbf{q^+}\mathbf{p^+}\mathbf{q}^{-1}=\mathbf{q^+}(\mathbf{q}^{-1})^\oplus\mathbf{p}

q+(q−1)⊕=[s−vTvsI+v∧][svT−vsI+v∧]=[100TvvT+s2I+2sv∧+(v∧)2]\mathbf{q^+}(\mathbf{q}^{-1})^\oplus=\begin{bmatrix} s&-\mathbf{v}^T\\ \mathbf{v}&s\mathbf{I}+\mathbf{v}^\land \end{bmatrix} \begin{bmatrix} s&\mathbf{v}^T\\ -\mathbf{v}&s\mathbf{I}+\mathbf{v}^\land \end{bmatrix} = \begin{bmatrix} 1&\mathbf{0}\\ \mathbf{0}^T&\mathbf{vv^T}+s^2\mathbf{I}+2s\mathbf{v^\land}+(\mathbf{v}^\land)^2\\ \end{bmatrix}

由此，

R=vvT+s2I+2sv∧+(v∧)2\mathbf{R}=\mathbf{vv^T}+s^2\mathbf{I}+2s\mathbf{v^\land}+(\mathbf{v}^\land)^2

对上式求迹，得到

tr(R)=tr(vvT)+3s2+2s⋅0+tr((v∧)2)=v12+v22+v32+3s2−2(v12+v22+v32)=(1−s2)+3s2−2(1−s2)=4s2−1\begin{aligned} tr(\mathbf{R})&=tr(\mathbf{vv^T})+3s^2+2s\cdot 0+tr((\mathbf{v^\land})^2)\\ &=v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2)\\ &=(1-s^2)+3s^2-2(1-s^2)\\ &=4s^2-1\\ \end{aligned}

θ=arccos⁡tr(R)−12=arccos⁡(2s2−1)\begin{aligned} \theta&=\arccos\frac{tr(\mathbf{R})-1}{2}\\ &=\arccos(2s^2-1) \end{aligned}

cos⁡θ=2s2−1=2cos⁡2θ2−1⇒θ=2arccos⁡s\cos\theta=2s^2-1=2\cos^2\frac{\theta}{2}-1\\ \Rightarrow \theta=2\arccos s

四元数到旋转向量的转换公式为

{θ=2arccos⁡q0[nx,ny,nz]T=[q1,q2,q3]Tsin⁡θ2\left\{ \begin{matrix} \theta = 2\arccos q_0\\ [n_x,n_y,n_z]^T=\dfrac{[q_1,q_2,q_3]^T}{\sin{\frac{\theta}{2}}} \end{matrix} \right.

其中，

sin⁡θ2=q12+q22+q32\sin\frac{\theta}{2}=\sqrt{q_1^2+q_2^2+q_3^2}

相似、仿射、射影变换

  1. 相似变换

    允许对物体进行均匀缩放

    TS=[sRt0T1]\mathbf{T}_S=\begin{bmatrix} s\mathbf{R}&\mathbf{t}\\ \mathbf{0}^T&1\\ \end{bmatrix}

    三维相似变换的集合称为相似变换群，记作Sim(3)Sim(3)。

  2. 仿射变换

    TA=[At0T1]\mathbf{T}_A=\begin{bmatrix} \mathbf{A}&\mathbf{t}\\ \mathbf{0}^T&1\\ \end{bmatrix}

    只要求A\mathbf{A}是可逆矩阵。仿射变换后，保证每个平行四边形面是平行四边形。

  3. 射影变换

    最一般的变换

    TP=[AtaTv]\mathbf{T}_P=\begin{bmatrix} \mathbf{A}&\mathbf{t}\\ \mathbf{a}^T&v\\ \end{bmatrix}

    保证平面相交相切不变。

李群、李代数

李群李代数基础

群

群是一种集合加上一种运算的代数结构。把集合记作A，运算记作·，那么群可以记作G=(A,⋅)G=(A,\cdot)。群要求运算满足以下条件：

  1. 封闭性：∀a1,a2∈A,a1⋅a2∈A\forall a_1,a_2\in A,\quad a_1\cdot a_2\in A
  2. 结合律：∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)\forall a_1,a_2,a_3\in A,\quad(a_1\cdot a_2)\cdot a_3=a_1\cdot(a_2\cdot a_3)
  3. 幺元：∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a\exists a_0\in A,\quad s.t.\quad\forall a\in A,\quad a_0\cdot a=a\cdot a_0=a
  4. 逆：∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0\forall a\in A,\quad \exists a^{-1}\in A,\quad s.t.\quad a\cdot a^{-1}=a_0

李群是指具有连续（光滑）性质的群。

李代数引出

RRT=I⇒R(t)R(t)T=I⇒R˙(t)R(t)T+R(t)R˙(t)T=0⇒R˙(t)R(t)T=−(R˙(t)R(t)T)T\mathbf{RR^T}=\mathbf{I}\\ \Rightarrow \mathbf{R}(t)\mathbf{R}(t)^T=\mathbf{I}\\ \Rightarrow\dot{\mathbf{R}}(t)\mathbf{R}(t)^T+\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=\mathbf{0}\\ \Rightarrow \dot{\mathbf{R}}(t)\mathbf{R}(t)^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t)^T)^T

可见，R˙(t)R(t)T\dot{\mathbf{R}}(t)\mathbf{R}(t)^T是一个反对称矩阵。

若

a∧=A\mathbf{a}^\land=\mathbf{A}

记

A∨=a\mathbf{A}^\vee=\mathbf{a}

对于R˙(t)R(t)T\dot{\mathbf{R}}(t)\mathbf{R}(t)^T，我们可以找到一个ϕ(t)∈R3\phi(t)\in\mathbb{R}^3与之对应

R˙(t)R(t)T=ϕ(t)∧\dot{\mathbf{R}}(t)\mathbf{R}(t)^T=\phi(t)^\land

右乘R(t)\mathbf{R}(t)，得到

R˙(t)=ϕ(t)∧R(t)\dot{\mathbf{R}}(t)=\phi(t)^\land\mathbf{R}(t)

可见，每对R(t)\mathbf{R}(t)求一次导，只需要左乘一个ϕ(t)∧\phi(t)^\land，考虑t0=0t_0=0时，设此时R(0)=I\mathbf{R}(0)=\mathbf{I}。把R(t)\mathbf{R}(t)在t=0t=0附近泰勒展开：

R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧I(t−0)=I+ϕ(t0)∧(t)\begin{aligned} \mathbf{R}(t)&\approx\mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)\\ &=\mathbf{I}+\phi(t_0)^\land \mathbf{I}(t-0)\\ &=I+\phi(t_0)^\land(t) \end{aligned}

由于ϕ\phi反映了R\mathbf{R}的导数性质，故称它在SO(3)SO(3)原点附近的正切空间上。同时在t0t_0附近，设ϕ\phi保持常数ϕ(t0)=ϕ0\phi(t_0)=\phi_0，那么有

R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t)\dot{\mathbf{R}}(t)=\phi(t_0)^\land\mathbf{R}(t)=\phi_0^\land\mathbf{R}(t)

这是一个关于R\mathbf{R}的微分方程，有初始值R(0)=I\mathbf{R}(0)=\mathbf{I}，解得

R(t)=eϕ0∧t\mathbf{R}(t)=e^{\phi_0^\land t}

旋转矩阵R\mathbf{R}与另一个反对称矩阵ϕ0∧t\phi_0^\land t通过指数关系发生了联系。

给定某个时刻的R\mathbf{R}，就能求得一个ϕ\phi，它描述了R\mathbf{R}在局部的导数关系。ϕ\phi正对应到SO(3)SO(3)上的李代数so(3)\mathfrak{so}(3)。

李代数的定义

每个李群都有对应的李代数李代数描述了李群的局部性质，是单位元附近的正切空间。一般李代数定义如下：

李代数由集合V\mathbb{V}、数域F\mathbb{F}和二元运算符[ ,]组成。如果满足以下性质，则(V,F,[,])(\mathbb{V},\mathbb{F},[,])构成一个李代数记作g\mathfrak{g}：

  1. 封闭性 ∀X,Y∈V,[X,Y]∈V\forall\mathbf{X},\mathbf{Y}\in\mathbb{V},[\mathbf{X},\mathbf{Y}]\in\mathbb{V}

  2. 双线性 ∀X,Y,Z∈V,a,b∈F\forall\mathbf{X},\mathbf{Y},\mathbf{Z}\in\mathbb{V},a,b\in\mathbb{F}，有

    [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y][a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}],\quad [\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]

  3. 自反性 ∀X∈V,[X,X]=0\forall\mathbf{X}\in\mathbb{V},[\mathbf{X},\mathbf{X}]=0

  4. 雅可比恒等式

    ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in\mathbb{V},[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=\mathbf{0}

二元运算称为李括号，李括号表达了两个元素的差异。

李代数so(3)\mathfrak{so}(3)

之前提到的ϕ\phi事实上是一种李代数。SO(3)SO(3)对应的李代数是定义在R3\mathbb{R}^3上的向量，记作ϕ\phi。每个ϕ\phi都可以生成一个反对称矩阵：

Φ=ϕ∧=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]∈R3×3\mathbf{\Phi}=\phi^\land=\begin{bmatrix} 0&-\phi_3&\phi_2\\ \phi_3&0&-\phi_1\\ -\phi_2&\phi_1&0\\ \end{bmatrix} \in\mathbb{R}^{3\times3}

两个向量ϕ1,ϕ2\phi_1,\phi_2对应的李括号为

[ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨[\phi_1,\phi_2]=(\mathbf{\Phi_1\Phi_2}-\mathbf{\Phi_2\Phi_1})^\vee

不引起歧义的情况下，so(3)\mathfrak{so}(3)的元素是三维向量或三维反对称矩阵，不加区别：

so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}\mathfrak{so}(3)=\{\phi\in\mathbb{R}^3,\mathbf{\Phi}=\phi^\land\in\mathbb{R}^{3\times3}\}

李代数se(3)\mathfrak{se}(3)

SE(3)SE(3)对应的李代数是定义在R6\mathbb{R}^6上的向量：

se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]∈R4×4}\mathfrak{se}(3)=\{\xi=\begin{bmatrix} \rho\\ \phi \end{bmatrix} \in\mathbb{R}^6, \rho\in\mathbb{R}^3, \phi\in\mathfrak{so}(3), \xi^\land= \begin{bmatrix} \phi^\land&\rho\\ \mathbf{0}^T&0\\ \end{bmatrix} \in\mathbb{R}^{4\times4}\}

李括号：

[ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨[\xi_1,\xi_2]=(\xi_1^\land\xi_2^\land-\xi_2^\land\xi_1^\land)^\vee

指数与对数映射

对so(3)\mathfrak{so}(3)中的任意元素ϕ\phi，定义指数映射：

exp⁡(ϕ∧)=∑n=0∞1n!(ϕ∧)n\exp(\phi^\land)=\sum^\infin_{n=0}\frac{1}{n!}(\phi^\land)^n

将ϕ\phi记作ϕ=∣ϕ∣ϕ^\phi=|\phi|\hat{\phi}，对于ϕ^∧\hat{\phi}^\land有：

ϕ^∧ϕ^∧=[−(ϕ^2)2−(ϕ^3)2ϕ^1ϕ^2ϕ^1ϕ^3ϕ^1ϕ^2−(ϕ^1)2−(ϕ^3)2ϕ^2ϕ^3ϕ^1ϕ^3ϕ^2ϕ^3−(ϕ^1)2−(ϕ^2)2]=ϕ^ϕ^T−I\hat{\phi}^\land\hat{\phi}^\land=\begin{bmatrix} -(\hat{\phi}_2)^2-(\hat{\phi}_3)^2&\hat{\phi}_1\hat{\phi}_2&\hat{\phi}_1\hat{\phi}_3\\ \hat{\phi}_1\hat{\phi}_2&-(\hat{\phi}_1)^2-(\hat{\phi}_3)^2&\hat{\phi}_2\hat{\phi}_3\\ \hat{\phi}_1\hat{\phi}_3&\hat{\phi}_2\hat{\phi}_3&-(\hat{\phi}_1)^2-(\hat{\phi}_2)^2&\\ \end{bmatrix} =\hat{\phi}\hat{\phi}^T-\mathbf{I}

以及

ϕ^∧ϕ^∧ϕ^∧=ϕ^∧(ϕ^ϕ^T−I)=−ϕ^∧\hat{\phi}^\land\hat{\phi}^\land\hat{\phi}^\land=\hat{\phi}^\land(\hat{\phi}\hat{\phi}^T-\mathbf{I})=-\hat{\phi}^\land

这样

exp⁡(ϕ∧)=exp⁡(∣ϕ∣ϕ^∧)=∑n=0∞1n!(∣ϕ∣ϕ^∧)n=I+∣ϕ∣ϕ^∧+12!(∣ϕ∣ϕ^∧)2+13!(∣ϕ∣ϕ^∧)3+14!(∣ϕ∣ϕ^∧)4+…=ϕ^ϕ^T−ϕ^∧ϕ^∧+∣ϕ∣ϕ^∧+12!∣ϕ∣2ϕ^∧ϕ^∧−13!∣ϕ∣3ϕ^∧−14!∣ϕ∣4ϕ^∧ϕ^∧+…=ϕ^ϕ^T+(∣ϕ∣−13!∣ϕ∣3+15!∣ϕ∣5−… )ϕ^∧−(1−12!∣ϕ∣2+14!∣ϕ∣4−… )ϕ^∧ϕ^∧=ϕ^∧ϕ^∧+I+sin⁡∣ϕ∣ϕ^∧−cos⁡∣ϕ∣ϕ^∧ϕ^∧=(1−cos⁡∣ϕ∣)ϕ^∧ϕ^∧+I+sin⁡∣ϕ∣ϕ^∧=cos⁡∣ϕ∣I+(1−cos⁡∣ϕ∣)ϕ^ϕ^T+sin⁡∣ϕ∣ϕ^∧\begin{aligned} \exp(\phi^\land)&=\exp(|\phi|\hat{\phi}^\land)=\sum^\infin_{n=0}\frac{1}{n!}(|\phi|\hat{\phi}^\land)^n\\ &=I+|\phi|\hat{\phi}^\land+\frac{1}{2!}(|\phi|\hat{\phi}^\land)^2+\frac{1}{3!}(|\phi|\hat{\phi}^\land)^3+\frac{1}{4!}(|\phi|\hat{\phi}^\land)^4+\dots\\ &=\hat{\phi}\hat{\phi}^T-\hat{\phi}^\land\hat{\phi}^\land+|\phi|\hat{\phi}^\land+\frac{1}{2!}|\phi|^2\hat{\phi}^\land\hat{\phi}^\land-\frac{1}{3!}|\phi|^3\hat{\phi}^\land-\frac{1}{4!}|\phi|^4\hat{\phi}^\land\hat{\phi}^\land+\dots\\ &=\hat{\phi}\hat{\phi}^T+(|\phi|-\frac{1}{3!}|\phi|^3+\frac{1}{5!}|\phi|^5-\dots)\hat{\phi}^\land-(1-\frac{1}{2!}|\phi|^2+\frac{1}{4!}|\phi|^4-\dots)\hat{\phi}^\land\hat{\phi}^\land\\ &=\hat{\phi}^\land\hat{\phi}^\land+I+\sin|\phi|\hat{\phi}^\land-\cos|\phi|\hat{\phi}^\land\hat{\phi}^\land\\ &=(1-\cos|\phi|)\hat{\phi}^\land\hat{\phi}^\land+I+\sin|\phi|\hat{\phi}^\land\\ &=\cos|\phi|I+(1-\cos|\phi|)\hat{\phi}\hat{\phi}^T+\sin|\phi|\hat{\phi}^\land \end{aligned}

注意到，这就是罗德里格斯公式。

R=cos⁡θI+(1−cos⁡θ)aaT+sin⁡θa∧\mathbf{R}=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{aa}^T+\sin\theta\mathbf{a}^\land

由此，so(3)\mathfrak{so}(3)实际上就是旋转向量组成的空间，指数映射对应罗德里格斯公式。反之，定义对数映射：

ϕ=ln⁡(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨\phi=\ln(\mathbf{R})^\vee=(\sum^\infin_{n=0}\frac{(-1)^n}{n+1}(\mathbf{R}-\mathbf{I})^{n+1})^\vee

可以用求角轴的方法求。

对于se(3)\mathfrak{se}(3)，这里直接给出公式

exp⁡(ξ∧)=[∑n=0∞1n!(ϕ∧)n∑n=0∞1(n+1)!(ϕ∧)nρ0T1]≜[RJρ0T1]=TJ=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧\begin{aligned} \exp(\xi^\land)&=\begin{bmatrix} \sum^\infin_{n=0}\dfrac{1}{n!}(\phi^\land)^n&\sum^\infin_{n=0}\dfrac{1}{(n+1)!}(\phi^\land)^n\rho\\ \mathbf{0}^T&1\\ \end{bmatrix}\\ &\triangleq \begin{bmatrix} \mathbf{R}&\mathbf{J\rho}\\ \mathbf{0}^T&1 \end{bmatrix} =T \end{aligned}\\ \mathbf{J}=\frac{\sin\theta}{\theta}\mathbf{I}+(1-\frac{\sin\theta}{\theta})\mathbf{a}\mathbf{a}^T+\frac{1-\cos\theta}{\theta}\mathbf{a}^\land

李代数求导与扰动模型

BCH公式与近似公式

BCH公式（Baker-Campbell-Hausdorff）完整形式见https://en.wikipedia.org/wiki/Baker%E2%80%93Campbell%E2%80%93Hausdorff_formula

这里只给出前几项：

ln⁡(exp⁡(A)exp⁡(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+…\ln(\exp(\mathbf{A})\exp(\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\dots

SO(3)SO(3)下的BCH近似为：

ln⁡(R1R2)∨=ln⁡(exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量Jl=J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧Jl−1=θ2cot⁡θ2I+(1−θ2cot⁡θ2)aaT−θ2a∧Jr(ϕ)=Jl(−ϕ)\ln(\mathbf{R_1}\mathbf{R_2})^\vee=\ln(\exp(\phi_1^\land)\exp(\phi_2^\land))^\vee\approx \left\{ \begin{matrix} \mathbf{J}_l(\phi_2)^{-1}\phi_1+\phi_2\quad当\phi_1为小量\\ \mathbf{J}_r(\phi_1)^{-1}\phi_2+\phi_1\quad当\phi_2为小量 \end{matrix} \right.\\ \mathbf{J}_l=\mathbf{J}=\frac{\sin\theta}{\theta}\mathbf{I}+(1-\frac{\sin\theta}{\theta})\mathbf{aa}^T+\frac{1-\cos\theta}{\theta}\mathbf{a}^\land\\ \mathbf{J}_l^{-1}=\frac{\theta}{2}\cot\frac{\theta}{2}\mathbf{I}+(1-\frac{\theta}{2}\cot\frac{\theta}{2})\mathbf{aa}^T-\frac{\theta}{2}\mathbf{a}^\land\\ \mathbf{J}_r(\phi)=\mathbf{J}_l(-\phi)

ΔR⋅R=exp⁡(Δϕ∧)exp⁡(ϕ∧)=exp⁡((ϕ+Jl−1(ϕ)Δϕ)∧)exp⁡((ϕ+Δϕ)∧)=exp⁡((JlΔϕ)∧)exp⁡(ϕ∧)=exp⁡(ϕ∧)exp⁡((JrΔϕ)∧)ΔT⋅T=exp⁡(Δξ∧)exp⁡(ξ∧)=exp⁡((ξ+Jl−1(ξ)Δξ)∧)T⋅ΔT=exp⁡(ξ∧)exp⁡(Δξ∧)=exp⁡((ξ+Jr−1(ξ)Δξ)∧)\Delta\mathbf{R}\cdot\mathbf{R}=\exp(\Delta\phi^\land)\exp(\phi^\land)=\exp((\phi+\mathbf{J}_l^{-1}(\phi)\Delta\phi)^\land)\\ \exp((\phi+\Delta\phi)^\land)=\exp((\mathbf{J}_l\Delta\phi)^\land)\exp(\phi^\land)=\exp(\phi^\land)\exp((\mathbf{J}_r\Delta\phi)^\land)\\ \Delta\mathbf{T}\cdot\mathbf{T}=\exp(\Delta\xi^\land)\exp(\xi^\land)=\exp((\xi+\mathcal{J}_l^{-1}(\xi)\Delta\xi)^\land)\\ \mathbf{T}\cdot\Delta\mathbf{T}=\exp(\xi^\land)\exp(\Delta\xi^\land)=\exp((\xi+\mathcal{J}_r^{-1}(\xi)\Delta\xi)^\land)\\

李代数求导

将一个空间点p\mathbf{p}旋转得到Rp\mathbf{Rp}。计算旋转之后点坐标相对于旋转的导，非正式记为

∂Rp∂R\frac{\partial\mathbf{Rp}}{\partial\mathbf{R}}

省略转置运算，上式就是计算

∂(exp⁡(ϕ∧)p)∂ϕ=lim⁡δϕ→0exp⁡((ϕ+δϕ)∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0exp⁡((Jlδϕ)∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0(I+(Jlδϕ)∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0(Jlδϕ)∧exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0−(exp⁡(ϕ∧)p)∧Jlδϕδϕ=−(Rp∧)Jl\begin{aligned} \frac{\partial(\exp(\phi^\land)\mathbf{p})}{\partial\phi}&=\lim_{\delta\phi\to\mathbf{0}}\frac{\exp((\phi+\delta\phi)^\land)\mathbf{p}-\exp(\phi^\land)\mathbf{p}}{\delta\phi}\\ &=\lim_{\delta\phi\to\mathbf{0}}\frac{\exp((\mathbf{J}_l\delta\phi)^\land)\exp(\phi^\land)\mathbf{p}-\exp(\phi^\land)\mathbf{p}}{\delta\phi}\\ &=\lim_{\delta\phi\to\mathbf{0}}\frac{(\mathbf{I}+(\mathbf{J}_l\delta\phi)^\land)\exp(\phi^\land)\mathbf{p}-\exp(\phi^\land)\mathbf{p}}{\delta\phi}\\ &=\lim_{\delta\phi\to\mathbf{0}}\frac{(\mathbf{J}_l\delta\phi)^\land\exp(\phi^\land)\mathbf{p}}{\delta\phi}\\ &=\lim_{\delta\phi\to\mathbf{0}}\frac{-(\exp(\phi^\land)\mathbf{p})^\land\mathbf{J}_l\delta\phi}{\delta\phi}\\ &=-(\mathbf{Rp}^\land)\mathbf{J}_l \end{aligned}

由此，

∂Rp∂R=−(Rp∧)Jl\frac{\partial\mathbf{Rp}}{\partial\mathbf{R}}=-(\mathbf{Rp}^\land)\mathbf{J}_l

扰动模型（左乘）

另一种求导方式是对R\mathbf{R}进行一次扰动ΔR\Delta\mathbf{R}，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，以左扰动为例。设左扰动ΔR\Delta\mathbf{R}对应的李代数为φ\varphi。然后，对φ\varphi求导，即

∂(Rp)∂φ=lim⁡φ→0exp⁡(φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=lim⁡φ→0(I+φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=lim⁡φ→0φ∧Rpφ=lim⁡φ→0−(Rp)∧φφ=−(Rp)∧\begin{aligned} \frac{\partial(\mathbf{Rp})}{\partial\varphi}&=\lim_{\varphi\to\mathbf{0}}\frac{\exp(\varphi^\land)\exp(\phi^\land)\mathbf{p}-\exp(\phi^\land)\mathbf{p}}{\varphi}\\ &=\lim_{\varphi\to\mathbf{0}}\frac{(\mathbf{I}+\varphi^\land)\exp(\phi^\land)\mathbf{p}-\exp(\phi^\land)\mathbf{p}}{\varphi}\\ &=\lim_{\varphi\to\mathbf{0}}\frac{\varphi^\land\mathbf{Rp}}{\varphi}\\ &=\lim_{\varphi\to\mathbf{0}}\frac{-(\mathbf{Rp})^\land\varphi}{\varphi}\\ &=-(\mathbf{Rp})^\land \end{aligned}

SE(3)SE(3)上的扰动模型

将一个空间点p\mathbf{p}变换得到Tp\mathbf{Tp}。给T\mathbf{T}左乘扰动ΔT=exp⁡(δξ∧)\Delta T=\exp(\delta\xi^\land)，设扰动项李代数为δξ=[δρ,δϕ]T\delta\xi=[\delta\rho,\delta\phi]^T，那么：

∂(Tp)∂δξ=[I−(Rp+t∧)0T0T]≜(Tp)⊙\frac{\partial(\mathbf{Tp})}{\partial\delta\xi}=\begin{bmatrix} \mathbf{I}&-(\mathbf{Rp}+\mathbf{t}^\land)\\ \mathbf{0}^T&\mathbf{0}^T\\ \end{bmatrix} \triangleq(\mathbf{Tp})^\odot

评估轨迹误差

估计轨迹Testi,i\mathbf{T}_{esti,i}，真实轨迹Tgt,i\mathbf{T}_{gt,i}，其中i=1,2,…,Ni=1,2,\dots,N

绝对轨迹误差（ATE）

ATEall=1N∑i=1N∣ln⁡(Tgt,i−1Testi,i)∨∣2ATE_{all}=\sqrt{\frac{1}{N}\sum^{N}_{i=1}|\ln(\mathbf{T}_{gt,i}^{-1}\mathbf{T}_{esti,i})^\vee|^2}

绝对平移误差

ATEtans=1N∑i=1N∣trans(Tgt,i−1Testi,i)∣2ATE_{tans}=\sqrt{\frac{1}{N}\sum^{N}_{i=1}|trans(\mathbf{T}_{gt,i}^{-1}\mathbf{T}_{esti,i})|^2}

transtrans表示只取平移部分

相对位姿误差（RPE）

考虑ii到i+Δti+\Delta t时刻的运动

RPEall=1N−Δt∑i=1N−Δt∣ln⁡((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∨∣2RPE_{all}=\sqrt{\frac{1}{N-\Delta t}\sum^{N-\Delta t}_{i=1}|\ln((\mathbf{T}_{gt,i}^{-1}\mathbf{T}_{gt,i+\Delta t})^{-1}(\mathbf{T}_{esti,i}^{-1}\mathbf{T}_{esti,i+\Delta t}))^\vee|^2}

相对平移误差

RPEtrans=1N−Δt∑i=1N−Δt∣trans((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∨∣2RPE_{trans}=\sqrt{\frac{1}{N-\Delta t}\sum^{N-\Delta t}_{i=1}|trans((\mathbf{T}_{gt,i}^{-1}\mathbf{T}_{gt,i+\Delta t})^{-1}(\mathbf{T}_{esti,i}^{-1}\mathbf{T}_{esti,i+\Delta t}))^\vee|^2}

相似变换群和李代数

相似变换

p′=[sRt0T1]=sRp+t\mathbf{p'}=\begin{bmatrix} s\mathbf{R}&\mathbf{t}\\ \mathbf{0}^T&1\\ \end{bmatrix} =s\mathbf{Rp}+\mathbf{t}

相似变换群

Sim(3)={S=[sRt0T1]∈R4×4}Sim(3)=\{\mathbf{S}=\begin{bmatrix} s\mathbf{R}&\mathbf{t}\\ \mathbf{0}^T &1\end{bmatrix} \in\mathbb{R}^{4\times4} \}

李代数

sim(3)={ζ∣ζ=[ρϕσ]∈R7,ζ∧=[σI+ϕ∧ρ0T0]∈R4×4}\mathfrak{sim}(3)=\{\zeta|\zeta=\begin{bmatrix} \rho\\ \phi\\ \sigma\\ \end{bmatrix} \in\mathbb{R}^7, \zeta^\land=\begin{bmatrix} \sigma\mathbf{I}+\phi^\land&\rho\\ \mathbf{0}^T&0\\ \end{bmatrix} \in\mathbb{R}^{4\times4} \}

指数映射

exp⁡(ζ∧)=[eσexp⁡(ϕ∧)Jsρ0T1]Js=eσ−1σI+σeσsin⁡θ+(1−eσcos⁡θ)θσ2+θ2a∧+(eσ−1σ+(eσcos⁡θ−1)σ+(eσsin⁡θ)θσ2+θ2)a∧a∧\exp(\zeta^\land)=\begin{bmatrix} e^\sigma\exp(\phi^\land)& \mathbf{J}_s\rho\\ \mathbf{0}^T&1\\ \end{bmatrix}\\ \mathbf{J}_s=\frac{e^{\sigma}-1}{\sigma}\mathbf{I}+\frac{\sigma e^\sigma\sin\theta+(1-e^\sigma\cos\theta)\theta}{\sigma^2+\theta^2}\mathbf{a}^\land+(\frac{e^{\sigma}-1}{\sigma}+\frac{(e^\sigma\cos\theta-1)\sigma+(e^\sigma\sin\theta)\theta}{\sigma^2+\theta^2})\mathbf{a}^\land\mathbf{a}^\land

扰动模型

∂Sp∂ζ=[I−q∧q0T0T0]\frac{\partial \mathbf{Sp}}{\partial\zeta}=\begin{bmatrix} \mathbf{I}&-\mathbf{q}^\land&\mathbf{q}\\ \mathbf{0}^T&\mathbf{0}^T&0 \end{bmatrix}

相机与图像

相机模型

针孔相机模型

image-20241114003737374

P[X,Y,Z]T→P′[X′,Y′,Z′]TP[X,Y,Z]^T\rightarrow P'[X',Y',Z']^T，设小孔焦距ff，得到

Zf=−XX′=−YY′\frac{Z}{f}=-\frac{X}{X'}=-\frac{Y}{Y'}

去掉负号，得到

Zf=XX′=YY′⇒{X′=fXZY′=fYZ\frac{Z}{f}=\frac{X}{X'}=\frac{Y}{Y'}\\ \Rightarrow\left\{ \begin{matrix} X'=f\dfrac{X}{Z}\\ Y'=f\dfrac{Y}{Z}\\ \end{matrix} \right.

传感器将感受到的光线转换成图像像素。在成像平面上固定一个像素平面o−u−vo-u-v，得到P′P'的像素坐标[u,v]T[u,v]^T。uu轴向右，vv轴向下。设像素坐标在uu轴上缩放α\alpha倍，在vv轴上缩放β\beta倍，原点平移[cx,cy]T[c_x,c_y]^T，得到：

{u=αX′+cxv=βY′+cy⇒{u=fxX′Z+cxv=fyY′Z+cy\left\{ \begin{matrix} u=\alpha X'+c_x\\ v=\beta Y'+c_y\\ \end{matrix} \right.\\ \Rightarrow \left\{ \begin{matrix} u=f_x\dfrac{X'}{Z}+c_x\\ v=f_y\dfrac{Y'}{Z}+c_y\\ \end{matrix} \right.\\

Z[uv1]=[fx0cx0fycy001][XYZ]≜KPZ\begin{bmatrix} u\\ v\\ 1\\ \end{bmatrix} = \begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} X\\ Y\\ Z\\ \end{bmatrix} \triangleq KP

KK称为相机内参数矩阵，确定相机内参称为标定。

PP点的世界坐标PwP_w，位姿由旋转矩阵RR和平移向量tt描述。

ZPuv=Z[uv1]=K(RPw+t)=KTPwZP_{uv}=Z \begin{bmatrix} u\\ v\\ 1\\ \end{bmatrix} =K(RP_w+t)=KTP_w

R,tR,t称为相机外参数。

归一化：

(RPw+t)=[X,Y,Z]T→[XZ,YZ,1]T(RP_w+t)=[X,Y,Z]^T\rightarrow[\frac{X}{Z},\frac{Y}{Z},1]^T

点的深度在投影过程中丢失了。

畸变

由透镜引起的畸变称为径向畸变，分为桶形畸变和枕形畸变。

image-20241114005502770

相机组装过程中不能使透镜和成像面严格平行，引入切向畸变。

image-20241114005645842

对于相机坐标系中的一点PP，我们能够通过5个畸变系数找到这个点在像素平面上的正确位置：

  1. 将三维空间点投影到归一化图像平面。设它的归一化坐标为[x,y]T[x,y]^T

  2. 对归一化平面上的点计算径向畸变和切向畸变。

    {xdistorted=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ydistorted=y(1+k1r2+k2r4+k3r6)+2p2xy+p1(r2+2y2)\left\{ \begin{matrix} x_{distorted}=x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2)\\ y_{distorted}=y(1+k_1r^2+k_2r^4+k_3r^6)+2p_2xy+p_1(r^2+2y^2)\\ \end{matrix} \right.

  3. 将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。

    {u=fxxdistorted+cxv=fyydistorted+cy\left\{ \begin{matrix} u=f_xx_{distorted}+c_x\\ v=f_yy_{distorted}+c_y\\ \end{matrix} \right.

单目相机的成像过程：

  1. 世界坐标系下有一个固定的点PP，世界坐标为PwP_w。
  2. 由于相机在运动，它的运动由R,tR,t或变换矩阵T∈SE(3)T\in SE(3)描述。PP的相机坐标为P~c=RPw+t\tilde{P}_c=RP_w+t。
  3. 这时的P~c\tilde{P}_c的分量为X,Y,ZX,Y,Z，把它们投影到归一化平面Z=1Z=1上，得到PP的归一化坐标：Pc=[X/Z,Y/Z,1]TP_c=[X/Z,Y/Z,1]^T。
  4. 有畸变时，根据畸变参数计算PcP_c。发生畸变后的坐标。
  5. PP的归一化坐标经过内参后，对应到它的像素坐标：Puv=KPcPuv=KP_c。

双目相机模型

image-20241114011014634

image-20241114011029411

z−fz=fracb−uL+uRb⇒z=fbd,d≜uL−uR\frac{z-f}{z}=frac{b-u_L+u_R}{b}\\ \Rightarrow z=\frac{fb}{d},\quad d\triangleq u_L-u_R

bb——基线，dd——视差

RGB-D相机模型

红外结构光

飞行时间

image-20241114011331375