【视觉SLAM入门】2.旋转 |
您所在的位置:网站首页 › 位移公式的导数是什么 › 【视觉SLAM入门】2.旋转 |
"川泽纳污"
0. 一个例子1. 群和李群2. 李代数2.1 推导和性质2.2
s
o
(
3
)
\mathscr{so(3)}
so(3) 和
s
e
(
3
)
\mathscr{se(3)}
se(3)2.3 计算李代数的幂
e
x
p
(
ϕ
\;exp(\phi
exp(ϕ^
)
)
)2.4 李代数乘法2.5 从李代数乘法到导数:2.5.1 直接求导2.5.2 扰动模型求导(常用)
3.
S
i
m
(
3
)
Sim(3)
Sim(3)与李代数
注意: Λ \boldsymbol {^\Lambda} Λ 记做“向量拉伸成矩阵”,(见外积) V \boldsymbol {^V} V 记做“矩阵坍缩成向量” 0. 一个例子今天所做的一切都是为了求解下边这个问题展开的! \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 最终要求解的是目标函数 J \boldsymbol{J} J关于变换矩阵 T \boldsymbol T T的导数我们经常会构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值 1. 群和李群 特殊正交群与特殊欧式群: S O ( 3 ) \qquad \qquad \qquad SO(3) SO(3) = { R ∈ R 3 x 3 ∣ R R T = I , d e t ( R ) = 1 R \in \R^{3x3} | RR^T=I, det(R)=1 R∈R3x3∣RRT=I,det(R)=1}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 )+ 一种代数结构(
记作
.
记作 .
记作.)。 群有四个性质,分别是 “封闭性、结合律、幺元、逆” 李群: S O ( 3 ) SO(3) SO(3)对乘法封闭,表示旋转,且连续,因为在实数空间物体的旋转是连续的。所以它是一种李群。 严谨地,李群是指具有连续(光滑)性质的群。 由于 S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)这两个李群对SLAM较为重要,主要讨论着两个李群。 2. 李代数 2.1 推导和性质从 R R R为正交阵且 R R T = I RR^T=I RRT=I开始推导,如下(笔记潦草,勿怪):
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 ϕ实际反应的导数的性质。 特别地,研究初始时刻 t 0 = 0 \;t_0=0 t0=0 , R ( 0 ) = I \;R(0)=I R(0)=I: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) 至此, ϕ \phi ϕ就正是对应 S O ( 3 ) \;SO(3) SO(3) 上的李代数 s o ( 3 ) \; \mathscr{so(3)} so(3)。旋转矩阵R与李代数 ϕ 0 \phi_0 ϕ0 通过指数关系发生了联系,反映了R在局部的导数关系。李代数: 记作 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的线性近似表达如下:
SE(3)类似: \qquad 参考前边的例子:我们知道,最终要求解的是目标函数 J \boldsymbol{J} J关于变换矩阵 T \boldsymbol T T的导数。从上边所有的铺垫到现在,有两种方法求解导数,分别介绍: 2.5.1 直接求导对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 第二行中省略了很多推导步骤,包括导数定义展开、BCH线性近似、泰勒展开舍去高阶项近似、将反对称符号看做叉积,交换后变号。 2.5.2 扰动模型求导(常用)对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的矩阵 3. S i m ( 3 ) Sim(3) Sim(3)与李代数\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} 同样的,尽管多了一个维度,它仍然具有李代数(7维),指数映射,对数映射, J J J , 导数(扰动模型),以后做单目我再自行了解吧 。 |
今日新闻 |
推荐新闻 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |