今天所做的一切都是为了求解下边这个问题展开的! \textcolor{blue}{今天所做的一切都是为了求解下边这个问题展开的!} 今天所做的一切都是为了求解下边这个问题展开的!
\qquad 机器人位姿 T \;T T ,观察到世界坐标系中的点 p \;p p,产生观测数据 z , 误差为 w 或 e \;z, 误差为w或e z,误差为w或e
z = T p + w \qquad\qquad\qquad\qquad\qquad\qquad z = Tp +w z=Tp+w
\qquad 共有N个这样的观测数据和路标,我们的任务是寻找最优的T,使误差最小化
min T J ( T ) = ∑ i = 1 N ∣ ∣ z i − T p i ∣ ∣ 2 2 \qquad\qquad\qquad\qquad\qquad\qquad \min\limits_TJ(\boldsymbol{T})=\sum\limits_{i=1}\limits^N||\boldsymbol{z_i-Tpi}||^2_2 TminJ(T)=i=1∑N∣∣zi−Tpi∣∣22
S E ( 3 ) \quad \qquad \qquad SE(3) SE(3) = { T = [ R t 0 T 1 ] ∈ R 4 x 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 T= \begin{bmatrix} \quad R&t&\\\quad0^T&1 \end{bmatrix} \in \R^{4x4} | R \in SO(3), t \in \R^3 T=[R0Tt1]∈R4x4∣R∈SO(3),t∈R3}
不难发现, R 和 T R和T R和T 对加法不封闭,即 R 1 + R 2 ∉ S O ( 3 ) R_1+R_2 \notin SO(3) R1+R2∈/SO(3)。
群: 只有一个运算的集合,即一种集合( 记作 A 记作\; A 记作A )+ 一种代数结构( 记作 . 记作 . 记作.)。
群有四个性质,分别是 “封闭性、结合律、幺元、逆”
常见的群:一般线性群 G L ( n ) \;GL(n) GL(n)(对乘法), 特殊正交群 S O ( n ) \; SO(n) SO(n) ,特殊欧式群 S E ( n ) \;SE(n) SE(n)
李群: S O ( 3 ) SO(3) SO(3)对乘法封闭,表示旋转,且连续,因为在实数空间物体的旋转是连续的。所以它是一种李群。 严谨地,李群是指具有连续(光滑)性质的群。
从 R R R为正交阵且 R R T = I RR^T=I RRT=I开始推导,如下(笔记潦草,勿怪):
\qquad\qquad\qquad 这里,可以知道 R ( t ) R ( t ) T = ϕ ( t ) R(t)R(t)^T=\phi(t) R(t)R(t)T=ϕ(t)^为反对称矩阵,上边记得 ^ 和v,分别表示把向量张成反对称矩阵(参考外积)和它的反向操作,接着对 R R R的导数进行研究
以上可以看出,每次求导,仅需要左乘一个 ϕ ( t ) \; \phi(t) ϕ(t)^ 即可。
将R(t)进行泰勒展开:
R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) \qquad\qquad\qquad\qquad\qquad R(t) \approx R(t_0)+\dot{R}(t_0)(t-t_0) = I + \phi(t_0) R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)^ ( t ) (t) (t)
观察可得,这里 ϕ \phi ϕ实际反应的导数的性质。
R ˙ ( t ) = ϕ ( t 0 ) \qquad\qquad\qquad\qquad \dot{R}(t) = \phi(t_0) R˙(t)=ϕ(t0)^ R ( t ) = ϕ 0 R(t)=\phi_0 R(t)=ϕ0^ R ( t ) R(t) R(t)
解这个(一阶常线性)微分方程得 R ( t ) = e x p ( ϕ 0 \qquad R(t) = exp(\phi_0 R(t)=exp(ϕ0^ t ) t) t)
李代数: 记作 g \;\mathscr {g} g 描述了李群的局部性质,通用李代数定义和性质如下。定义中的二元运算被称为李括号。如,三维向量 R 3 R_3 R3上定义的叉积就是一种李代数 g = ( R 3 , R , \; \mathscr{g} = (\R^3,\R, g=(R3,R,X ) ) )。
so(3):三维
\qquad 因为 ϕ \;\phi ϕ 本来代表向量,但是每个R都可以生成一个矩阵,它们关系紧密,后文不区分它是v后的矩阵,还是^后的向量,择适者用。
\qquad s o ( 3 ) \mathscr{so(3)} so(3) 表示一个由三维向量组成的集合,每个向量对应到一个反对称矩阵,可以表达旋转矩阵的导数。它与 SO(3) 的关系由指数映射给定:
如下
R = e x p ( ϕ \qquad\qquad\qquad\qquad\qquad R=exp(\phi R=exp(ϕ^ ) ) )
它的李括号运算: [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) V [\phi_1,\phi_2] = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^V [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)V
se(3):六维
也表示类似局部导数的性质,但是下文的v和^仅仅指代向量和矩阵的互换,与反对称无关。
定义如下:
so(3)的映射:
\qquad 这里称计算 e x p ( ϕ \;exp(\phi exp(ϕ^ ) ) ) 的步骤为指数映射(Exponential Map)
矩阵指数映射的计算公式: e x p ( ϕ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ) n \quad exp(\phi) = \sum\limits_{n=0}\limits^\infty{\frac{1}{n!}(\phi)^n} exp(ϕ)=n=0∑∞n!1(ϕ)n
定义 ϕ = θ a , a \phi=\theta a, a ϕ=θa,a是方向向量,模长1。推导可得:
和罗德里格斯公式一模一样,侧面反映了李代数 ≈ \approx ≈旋转向量,而李群是旋转矩阵。用迹则可以求 θ \;\theta θ 和 a \; a a,在 θ ∈ [ − π , π ] \;\theta \in [-\pi, \pi] θ∈[−π,π] 内,是一一对应的。
se(3)的映射:
\quad 由上述内容,和so(3)推导一致可得
ξ Λ = [ ϕ Λ ρ 0 T 1 ] 故 e x p ( ξ Λ ) = [ R J ρ 0 T 1 ] \xi^\Lambda = \begin{bmatrix} \phi^\Lambda & \rho \\0^T &1 \end{bmatrix} \qquad故\qquad exp(\xi^\Lambda) = \begin{bmatrix} R & J\rho \\0^T &1 \end{bmatrix} ξΛ=[ϕΛ0Tρ1]故exp(ξΛ)=[R0TJρ1]
\qquad 注意:旋转部分指数映射同 s o ( 3 ) \boldsymbol {so(3)} so(3),(不同才搞笑了)平移部分在做指数映射后多了一项系数矩阵 J \;\boldsymbol {J} J ,也有点像罗德里格斯公式(仍然设 ϕ = θ a \;\phi = \theta a ϕ=θa):
SO(3):
\qquad 李群乘法即李代数加法,但是矩阵的指数映射不满足常数的 e a ⋅ e b = e ( a + b ) e^a \cdot e^b = e^{(a+b)} ea⋅eb=e(a+b)
\qquad 求解李代数指数映射乘积还得看BCH公式(Baker-Campbell-Hausdorff),如下:
l n ( e A ⋅ e B ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + . . . . . . \qquad\qquad\qquad ln(e^A\cdot e^B)=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...... ln(eA⋅eB)=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+......
其中的 [ ] 表示李括号,当A或B
为极小量的时候,BCH的线性近似表达如下:
- 直观的理解上式:李群的R1和R2左乘近似(R1小)或右乘近似(R2小)而已。其实就是 Δ R ⋅ R \Delta R \cdot R ΔR⋅R或 R ⋅ Δ R \ R \cdot \Delta R R⋅ΔR。一个微小位移。
\qquad 我们以左乘为例计算一下 J l − 1 \;J_l^{-1} Jl−1:
\qquad 参考前边的例子:我们知道,最终要求解的是目标函数 J \boldsymbol{J} J关于变换矩阵 T \boldsymbol T T的导数。从上边所有的铺垫到现在,有两种方法求解导数,分别介绍:
对SO(3)
空间点p经过旋转
\qquad 便于理解的记法,设 R R R对应的李代数为 ϕ \phi ϕ:
∂ ( R p ) ∂ R = ∂ ( e x p ( ϕ Λ ) p ) ∂ ϕ ⟹ − ( R p ) Λ J l \frac {\partial(Rp)} {\partial R} =\frac {\partial(exp(\phi^{\Lambda})p)} {\partial \phi} \\ \quad \\ \Longrightarrow\ -(Rp)^\Lambda J_l ∂R∂(Rp)=∂ϕ∂(exp(ϕΛ)p)⟹ −(Rp)ΛJl
对SO(3)
空间点p的旋转可以看成一次左扰动
\qquad 扰动记作 Δ R \;\Delta R ΔR,李代数为 φ \; \varphi φ,则
∂ ( R p ) ∂ φ = lim φ → 0 e x p ( φ Λ ) e x p ( ϕ Λ ) p − e x p ( ϕ Λ ) p φ ⟹ − ( R p ) Λ \frac {\partial(Rp)} {\partial \varphi} =\lim_{\varphi \to 0} \frac {exp(\varphi^{\Lambda})exp(\phi^{\Lambda})p - exp(\phi^{\Lambda})p} {\varphi} \\ \quad \\ \Longrightarrow\ -(Rp)^\Lambda ∂φ∂(Rp)=φ→0limφexp(φΛ)exp(ϕΛ)p−exp(ϕΛ)p⟹ −(Rp)Λ
对SE(3)
直接上扰动 Δ T = e x p ( δ ξ Λ ) \;\Delta \boldsymbol T = exp(\delta \xi ^\Lambda) ΔT=exp(δξΛ),它的李代数 δ ξ = [ δ ρ , δ ϕ ] T \;\delta\boldsymbol \xi = [\delta \boldsymbol \rho, \delta \phi]^T δξ=[δρ,δϕ]T
直接上结果
∂ ( T p ) ∂ δ ξ = [ I − ( R p + t ) Λ 0 T 0 T ] = △ ( T p ) ⊙ \frac {\partial (Tp)}{\partial \delta \xi } = \begin{bmatrix} I&-(Rp+t)^\Lambda \\0^T & 0^T \end{bmatrix} =^{\triangle} (Tp)^\odot ∂δξ∂(Tp)=[I0T−(Rp+t)Λ0T]=△(Tp)⊙
其中 ⊙ \odot ⊙表示,一个齐次坐标展开成4X6的矩阵
\qquad 相似变换群( S i m Sim Sim),用于解决单目SLAM中的尺度问题,在这种情况下会显式表示出尺度 s \;s s,此时相机坐标系下的点经过相似变换(不是欧式变换):
描述该过程如下:
p ′ = [ s R t 0 T 1 ] p = s R p + t p' = \begin{bmatrix} \boldsymbol{sR}&t \\0^T&1 \end{bmatrix}p = s\boldsymbol{Rp+t} p′=[sR0Tt1]p=sRp+t
而 S i m ( 3 ) Sim(3) Sim(3)如下:
S i m ( 3 ) = { [ S = s R t 0 T 1 ] ∈ R 4 × 4 } Sim(3) =\begin{Bmatrix} \begin{bmatrix}\boldsymbol S= \begin{matrix} \boldsymbol{sR}&t \\0^T&1 \end{matrix} \end{bmatrix}\in \R^{4\times4}\end{Bmatrix} Sim(3)={[S=sR0Tt1]∈R4×4}