技术标签: slam c++ 矩阵 视觉SLAM十四讲 线性代数
课上我们讲解了什么是群。请根据群定义,求解以下问题:
{ Z , + } \{\mathbb{Z}, + \} { Z,+}是否为群?若是,验证其满足群定义;若不是,说明理由。
根据群定义可知 { Z , + } \{\mathbb{Z}, + \} { Z,+}是群。证明如下:
封闭性: ∀ z 1 , z 2 ∈ Z , z 1 + z 2 ∈ Z . \forall z_1,z_2 \in \mathbb{Z},\space z_1 + z_2 \in \mathbb{Z}. ∀z1,z2∈Z, z1+z2∈Z.
结合律: ∀ z 1 , z 2 , z 3 ∈ Z , ( z 1 + z 2 ) + z 3 = z 1 + ( z 2 + z 3 ) . \forall z_1,z_2 ,z_3\in \mathbb{Z},\space (z_1 + z_2) + z_3 = z_1 + (z_2 + z_3). ∀z1,z2,z3∈Z, (z1+z2)+z3=z1+(z2+z3).
幺元: ∃ 0 ∈ Z , s . t . ∀ z ∈ Z , 0 + z = z + 0 = z . \exists \space 0 \in \mathbb{Z}, \space s.t. \forall z\in \mathbb{Z}, \space 0+z = z+0 = z. ∃ 0∈Z, s.t.∀z∈Z, 0+z=z+0=z.
逆: ∀ z ∈ Z , ∃ − z ∈ Z , s . t . z + ( − z ) = 0. \forall z \in \mathbb{Z}, \space \exists -z \in \mathbb{Z}, \space s.t. z+(-z) = 0. ∀z∈Z, ∃−z∈Z, s.t.z+(−z)=0.
{ N , + } \{\mathbb{N}, + \} { N,+}是否为群?若是,验证其满足群定义;若不是,说明理由。
根据群定义可知 { N , + } \{\mathbb{N}, + \} { N,+}不是群。证明如下:
封闭性: ∀ n 1 , n 2 ∈ N , n 1 + n 2 ∈ N . \forall n_1,n_2 \in \mathbb{N},\space n_1 + n_2 \in \mathbb{N}. ∀n1,n2∈N, n1+n2∈N.
结合律: ∀ n 1 , n 2 , n 3 ∈ N , ( n 1 + n 2 ) + n 3 = n 1 + ( n 2 + n 3 ) . \forall n_1,n_2 ,n_3\in \mathbb{N},\space (n_1 + n_2) + n_3 = n_1 + (n_2 + n_3). ∀n1,n2,n3∈N, (n1+n2)+n3=n1+(n2+n3).
幺元: ∃ 0 ∈ N , s . t . ∀ n ∈ N , 0 + n = n + 0 = n . \exists \space 0 \in \mathbb{N}, \space s.t. \forall n\in \mathbb{N}, \space 0+n = n+0 = n. ∃ 0∈N, s.t.∀n∈N, 0+n=n+0=n.
但是不满足逆的性质,对于任意的 n ∈ N n \in \mathbb{N} n∈N无法找到其逆 n − 1 n^{-1} n−1,使得 n + n − 1 = 0 n+n^{-1} = 0 n+n−1=0。
解释什么是阿贝尔群。并说明矩阵及乘法构成的群是否为阿贝尔群。
阿贝尔群(Abel Group)又称交换群或可交换群,它由自身的集合G和二元运算*构成,它除了满足一般的群公理之外,还满足交换律。因为阿贝尔群的群运算满足交换律和结合律,群元素乘积的值与乘法运算时的次序无关。
矩阵乘法构成的群不是阿贝尔群,因为矩阵乘法不满足交换律。
解题思路,将括号部分带入性质定义。
封闭性:集合中任意两元素通过李括号运算得到的结果仍属于集合, a × b a \times b a×b得到的结果仍是一个三维向量。
即 ∀ X , Y ∈ R 3 , 有 X × Y ∈ R 3 . \forall \space X,Y \in \mathbb{R}^3, 有 X \times Y \in \mathbb{R}^3. ∀ X,Y∈R3,有X×Y∈R3.
双线性:对于 ∀ X , Y , Z ∈ R 3 , a , b ∈ R \forall \space X,Y,Z\in \mathbb{R}^3, a,b \in \mathbb{R} ∀ X,Y,Z∈R3,a,b∈R,向量叉乘运算满足乘法分配律,因此可得,
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY, Z] = a[X,Z]+b[Y,Z], [Z,aX+bY] = a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]满足双线性。
自反性:因为向量和自己叉乘结果为0,集合中元素和自己通过李括号运算结果为0,因此满足自反性。
雅克比等价:对于 ∀ X , Y , Z ∈ R 3 , \forall \space X,Y,Z\in \mathbb{R}^3, ∀ X,Y,Z∈R3,有
$ [X,[Y,Z]] + [Y,[Z,X]]+[Z,[X,Y]] = X\times (Y\times Z)+Y\times (Z\times X)+Z\times (X\times Y)$
根据拉格朗日公式 X × ( Y × Z ) = Y ( X ⋅ Z ) − Z ( X ⋅ Y ) X\times (Y\times Z) = Y(X\cdot Z)- Z(X\cdot Y) X×(Y×Z)=Y(X⋅Z)−Z(X⋅Y)代入上式可得:
[ X , [ Y , Z ] ] + [ Y , [ Z , X ] ] + [ Z , [ X , Y ] ] = X × ( Y × Z ) + Y × ( Z × X ) + Z × ( X × Y ) = Y ( X ⋅ Z ) − Z ( X ⋅ Y ) + Z ( Y ⋅ X ) − X ( Y ⋅ Z ) + X ( Z ⋅ Y ) − Y ( Z ⋅ X ) = 0 \begin{aligned}[X,[Y,Z]] + [Y,[Z,X]]+[Z,[X,Y]] &= X\times (Y\times Z)+Y\times (Z\times X)+Z\times (X\times Y)\\ &=Y(X\cdot Z)- Z(X\cdot Y) + Z(Y\cdot X)- X(Y\cdot Z) + X(Z\cdot Y)- Y(Z\cdot X)\\ &= 0 \end{aligned} [X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]=X×(Y×Z)+Y×(Z×X)+Z×(X×Y)=Y(X⋅Z)−Z(X⋅Y)+Z(Y⋅X)−X(Y⋅Z)+X(Z⋅Y)−Y(Z⋅X)=0
所以满足雅克比等价。
因此 g = ( R 3 , R , × ) \mathfrak{g} = (\mathbb{R}^3,\mathbb{R}, \times) g=(R3,R,×)构成李代数。
推导过程如下:
设 ξ = [ ρ , ϕ ] T ∈ s e ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 其 中 , [ ϕ ∧ ρ 0 T 0 ] 2 = [ ϕ ∧ ρ 0 T 0 ] × [ ϕ ∧ ρ 0 T 0 ] = [ ( ϕ ∧ ) 2 ϕ ∧ ρ 0 T 0 ] [ ϕ ∧ ρ 0 T 0 ] 3 = [ ϕ ∧ ρ 0 T 0 ] 2 × [ ϕ ∧ ρ 0 T 0 ] = [ ( ϕ ∧ ) 3 ϕ ∧ ϕ ∧ ρ 0 T 0 ] 因 此 , [ ϕ ∧ ρ 0 T 0 ] n = [ ϕ ∧ ρ 0 T 0 ] n × [ ϕ ∧ ρ 0 T 0 ] = [ ( ϕ ∧ ) n ( ϕ ∧ ) n − 1 ρ 0 T 0 ] 将 指 数 映 射 写 成 一 个 泰 勒 展 开 可 得 , exp ( ξ ∧ ) = ∑ n = 0 ∞ 1 n ! [ ϕ ∧ ρ 0 T 0 ] n = I + ∑ n = 1 ∞ 1 n ! [ ϕ ∧ ρ 0 T 0 ] n = I + [ ∑ n = 1 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 1 ∞ 1 n ! ( ϕ ∧ ) n − 1 ρ 0 T 0 ] = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] \begin{aligned} 设 \space \xi &= [\rho , \phi]^T \in \mathfrak{se}(3),\\ \xi^{\wedge} &= \begin{bmatrix} \phi^{\wedge}&\rho \\ 0^T & 0 \end{bmatrix} \in \mathbb{R}^{4\times 4} \\ 其中,\\ \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix}^{2} &= \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix} \times \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix} \\&= \begin{bmatrix} (\phi^{\wedge} )^2& \phi^{\wedge} \rho \\ 0^T & 0 \end{bmatrix} \\ \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix}^{3} &= \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix}^{2} \times \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix} \\&= \begin{bmatrix} (\phi^{\wedge} )^3& \phi^{\wedge} \phi^{\wedge} \rho \\ 0^T & 0 \end{bmatrix} \\ 因此,\\ \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix}^{n} &= \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix}^{n} \times \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix} \\&= \begin{bmatrix} (\phi^{\wedge} )^n & (\phi^{\wedge})^{n-1} \rho \\ 0^T & 0 \end{bmatrix} \\ 将指数映射写成一个泰勒展开可得,\\ \exp(\xi^{\wedge}) &= \sum_{n=0}^\infty \frac{1}{n!} \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix}^n \\ &= I + \sum_{n=1}^\infty \frac{1}{n!} \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^T & 0 \end{bmatrix} ^n \\ &= I + \begin{bmatrix} \sum_{n=1}^\infty \frac{1}{n!}(\phi^{\wedge})^n & \sum_{n=1}^\infty \frac{1}{n!}(\phi^{\wedge})^{n-1} \rho \\ 0^T & 0 \end{bmatrix} \\ &= \begin{bmatrix} \sum_{n=0}^\infty \frac{1}{n!}(\phi^{\wedge})^n & \sum_{n=0}^\infty \frac{1}{(n+1)!}(\phi^{\wedge})^{n} \rho \\ 0^T & 1 \end{bmatrix} \end{aligned} 设 ξξ∧其中,[ϕ∧0Tρ0]2[ϕ∧0Tρ0]3因此,[ϕ∧0Tρ0]n将指数映射写成一个泰勒展开可得,exp(ξ∧)=[ρ,ϕ]T∈se(3),=[ϕ∧0Tρ0]∈R4×4=[ϕ∧0Tρ0]×[ϕ∧0Tρ0]=[(ϕ∧)20Tϕ∧ρ0]=[ϕ∧0Tρ0]2×[ϕ∧0Tρ0]=[(ϕ∧)30Tϕ∧ϕ∧ρ0]=[ϕ∧0Tρ0]n×[ϕ∧0Tρ0]=[(ϕ∧)n0T(ϕ∧)n−1ρ0]=n=0∑∞n!1[ϕ∧0Tρ0]n=I+n=1∑∞n!1[ϕ∧0Tρ0]n=I+[∑n=1∞n!1(ϕ∧)n0T∑n=1∞n!1(ϕ∧)n−1ρ0]=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]
式(2)得证。
令 ϕ = θ a , 那 么 , ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n = ∑ n = 0 ∞ 1 ( n + 1 ) ! ( θ a ∧ ) n = I + 1 2 ! ( θ a ∧ ) + 1 3 ! ( θ a ∧ ) 2 + 1 4 ! ( θ a ∧ ) 3 + ⋯ = I + 1 2 ! θ a ∧ + 1 3 ! θ 2 ( a ∧ ) 2 + 1 4 ! θ 3 ( a ∧ ) 3 + ⋯ = I + ( 1 2 ! θ − 1 4 ! θ 3 + ⋯ ) a ∧ + ( 1 3 ! θ 2 − 1 5 ! θ 4 + ⋯ ) a ∧ a ∧ = I + 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 + ⋯ ) a ∧ + 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯ ) ( a a T − I ) 令 \phi = \theta a,那么,\\ \begin{aligned} \sum_{n=0}^\infty \frac{1}{(n+1)!}(\phi^{\wedge})^n &= \sum_{n=0}^\infty \frac{1}{(n+1)!}(\theta a^{\wedge})^n \\&= I + \frac{1}{2!}(\theta a^{\wedge}) + \frac{1}{3!}(\theta a^{\wedge})^2 + \frac{1}{4!}(\theta a^{\wedge}) ^3 + \cdots \\ &= I + \frac{1}{2!}\theta a^{\wedge} + \frac{1}{3!}\theta^2 (a^{\wedge})^2 + \frac{1}{4!}\theta^3 (a^{\wedge}) ^3 + \cdots \\ &= I + \begin{pmatrix} \frac{1}{2!}\theta - \frac{1}{4!}\theta^3 + \cdots \end{pmatrix} a^{\wedge} + \begin{pmatrix} \frac{1}{3!}\theta^2 - \frac{1}{5!}\theta^4 + \cdots \end{pmatrix} a^{\wedge} a^{\wedge} \\ &= I + \frac{1}{\theta} \begin{pmatrix} \frac{1}{2!}\theta^2 - \frac{1}{4!}\theta^4 + \cdots \end{pmatrix} a^{\wedge} + \frac{1}{\theta} \begin{pmatrix} \frac{1}{3!}\theta^3 - \frac{1}{5!}\theta^5 + \cdots \end{pmatrix} (aa^T - I) \\ \end{aligned} 令ϕ=θa,那么,n=0∑∞(n+1)!1(ϕ∧)n=n=0∑∞(n+1)!1(θa∧)n=I+2!1(θa∧)+3!1(θa∧)2+4!1(θa∧)3+⋯=I+2!1θa∧+3!1θ2(a∧)2+4!1θ3(a∧)3+⋯=I+(2!1θ−4!1θ3+⋯)a∧+(3!1θ2−5!1θ4+⋯)a∧a∧=I+θ1(2!1θ2−4!1θ4+⋯)a∧+θ1(3!1θ3−5!1θ5+⋯)(aaT−I)
根据泰勒展开式:
sin x = x − 1 3 ! x 3 + 1 5 ! x 5 + ⋯ cos x = 1 − 1 2 ! x 2 + 1 4 ! x 4 + ⋯ 1 − cos x = 1 2 ! x 2 − 1 4 ! x 4 + ⋯ \sin x = x - \frac{1}{3!}x^3 + \frac{1}{5!} x^5 + \cdots \\ \cos x = 1 - \frac{1}{2!}x^2 + \frac{1}{4!} x^4 + \cdots \\ 1-\cos x = \frac{1}{2!}x^2 - \frac{1}{4!} x^4 + \cdots \\ sinx=x−3!1x3+5!1x5+⋯cosx=1−2!1x2+4!1x4+⋯1−cosx=2!1x2−4!1x4+⋯
得
令 ϕ = θ a , , 其 中 , θ 为 模 长 , a 为 单 位 长 度 为 1 的 单 位 向 量 有 , a ∧ a ∧ = a a T − I a ∧ a ∧ a ∧ = − a ∧ ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n = ∑ n = 0 ∞ 1 ( n + 1 ) ! ( θ a ∧ ) n = I + 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 + ⋯ ) a ∧ + 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯ ) ( a a T − I ) = I + 1 − cos θ θ a ∧ + 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯ ) a a T − 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯ ) I = 1 − cos θ θ a ∧ + 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯ ) a a T + 1 θ ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 + ⋯ ) I = 1 − cos θ θ a ∧ + 1 θ ( θ − sin θ ) a a T + sin θ θ I ≜ J 令 \phi = \theta a,,\\ 其中,\theta为模长 , a为单位长度为1的单位向量\\ 有 ,a^{\wedge}a^{\wedge} = aa^T-I\\ a^{\wedge}a^{\wedge}a^{\wedge} = -a^{\wedge} \\ \begin{aligned} \sum_{n=0}^\infty \frac{1}{(n+1)!}(\phi^{\wedge})^n &= \sum_{n=0}^\infty \frac{1}{(n+1)!}(\theta a^{\wedge})^n \\&= I + \frac{1}{\theta} \begin{pmatrix} \frac{1}{2!}\theta^2 - \frac{1}{4!}\theta^4 + \cdots \end{pmatrix} a^{\wedge} + \frac{1}{\theta} \begin{pmatrix} \frac{1}{3!}\theta^3 - \frac{1}{5!}\theta^5 + \cdots \end{pmatrix} (aa^T - I) \\ &= I + \frac{1-\cos \theta}{\theta}a^{\wedge} + \frac{1}{\theta} \begin{pmatrix} \frac{1}{3!}\theta^3 - \frac{1}{5!}\theta^5 + \cdots \end{pmatrix} aa^T - \frac{1}{\theta} \begin{pmatrix} \frac{1}{3!}\theta^3 - \frac{1}{5!}\theta^5 + \cdots \end{pmatrix}I \\ &= \frac{1-\cos \theta}{\theta}a^{\wedge} + \frac{1}{\theta} \begin{pmatrix} \frac{1}{3!}\theta^3 - \frac{1}{5!}\theta^5 + \cdots \end{pmatrix} aa^T + \frac{1}{\theta} \begin{pmatrix} \theta - \frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5 + \cdots \end{pmatrix}I \\ &= \frac{1-\cos \theta}{\theta}a^{\wedge} + \frac{1}{\theta}(\theta-\sin \theta)aa^T + \frac{\sin\theta}{\theta}I \\ &\triangleq J \end{aligned} 令ϕ=θa,,其中,θ为模长,a为单位长度为1的单位向量有,a∧a∧=aaT−Ia∧a∧a∧=−a∧n=0∑∞(n+1)!1(ϕ∧)n=n=0∑∞(n+1)!1(θa∧)n=I+θ1(2!1θ2−4!1θ4+⋯)a∧+θ1(3!1θ3−5!1θ5+⋯)(aaT−I)=I+θ1−cosθa∧+θ1(3!1θ3−5!1θ5+⋯)aaT−θ1(3!1θ3−5!1θ5+⋯)I=θ1−cosθa∧+θ1(3!1θ3−5!1θ5+⋯)aaT+θ1(θ−3!1θ3+5!1θ5+⋯)I=θ1−cosθa∧+θ1(θ−sinθ)aaT+θsinθI≜J
式(3)得证。
证明
R a ∧ R T = ( R a ) ∧ R a^{\wedge}R^T = (Ra)^{\wedge} Ra∧RT=(Ra)∧:
设 , ∃ c = a ∧ b 则 , ∀ R ∈ S O ( 3 ) 有 R c = ( R a ) ∧ ( R b ) ⇒ c = R − 1 ( R a ) ∧ R b 因 为 c = a ∧ b 所 以 a ∧ = R − 1 ( R a ) ∧ R ⇒ R a ∧ R − 1 = ( R a ) ∧ R − 1 = R T 得 证 : R a ∧ R T = ( R a ) ∧ \begin{aligned} 设,\exists \space c &= a^{\wedge}b \\ 则,\forall R \in SO(3) \space 有 Rc &= (Ra)^{\wedge}(Rb) \\ \Rightarrow c &= R^{-1}(Ra)^{\wedge}Rb \\ 因为\space c &= a^{\wedge}b \\ 所以\space a^{\wedge} &= R^{-1}(Ra)^{\wedge}R \\ \Rightarrow R a^{\wedge}R^{-1} &= (Ra)^{\wedge} \\ R^{-1} &= R^T \\ 得证:R a^{\wedge}R^T &= (Ra)^{\wedge} \end{aligned} 设,∃ c则,∀R∈SO(3) 有Rc⇒c因为 c所以 a∧⇒Ra∧R−1R−1得证:Ra∧RT=a∧b=(Ra)∧(Rb)=R−1(Ra)∧Rb=a∧b=R−1(Ra)∧R=(Ra)∧=RT=(Ra)∧
证明 S O ( 3 ) SO(3) SO(3)的伴随性质:
参考资料:
https://math.stackexchange.com/questions/2190603/derivation-of-adjoint-for-so3
[1] Eade E. Lie groups for 2d and 3d transformations[J]. Revised Dec, 2013, 117: 118.
在上边链接中发现了居然还有这种性质,太好用了!发现自己就是小菜鸡!
对 R exp ( p ∧ ) R T 求 对 数 ⇒ ln ( R exp ( p ∧ ) R T ) = ln ( R exp ( p ∧ ) R − 1 ) 根 据 上 图 的 公 式 可 得 = R p ∧ R − 1 = R p ∧ R T = ( R p ) ∧ 对 等 式 两 边 求 指 数 可 得 : R exp ( p ∧ ) R T = exp ( ( R p ) ∧ ) 得 证 对 R \exp(p^{\wedge})R^T求对数\\ \begin{aligned} \Rightarrow \ln(R \exp(p^{\wedge}) R^T ) &= \ln(R \exp(p^{\wedge}) R^{-1} ) \\ 根据上图的公式可得 &= R p^{\wedge} R^{-1} \\ &= R p^{\wedge} R^T \\ &=(Rp)^{\wedge} \\ 对等式两边求指数可得:\\ R\exp(p^{\wedge})R^T &= \exp((Rp)^{\wedge}) \\ 得证 \end{aligned} 对Rexp(p∧)RT求对数⇒ln(Rexp(p∧)RT)根据上图的公式可得对等式两边求指数可得:Rexp(p∧)RT得证=ln(Rexp(p∧)R−1)=Rp∧R−1=Rp∧RT=(Rp)∧=exp((Rp)∧)
注:这一题的证明我走了一些弯路,在阅读了链接中的提示和文献后还是一头雾水,始终没想到怎么用上图中的公式。网上看到了有人是下图这样证明的,但我感觉这样的证明法在第三行直接引入 R ⋅ exp ( p ∧ ) = exp ( A d j R ⋅ p ∧ ) ⋅ R R\cdot \exp(p^{\wedge}) = \exp(Adj_{R\cdot p^{\wedge}})\cdot R R⋅exp(p∧)=exp(AdjR⋅p∧)⋅R,这一步我不太理解。希望助教老师如果时间充足的话可以帮忙看下,下图所示的证明方法是否正确?
img source:https://blog.csdn.net/jiahao62/article/details/80655542
旋转点对旋转的导数。请分别通过左、右扰动的方式,计算 ∂ R p ∂ R \frac{\partial Rp}{\partial R} ∂R∂Rp.
存在两种解决方法:
注:这里并不能按照矩阵微分来定义导数, ∂ R p ∂ R \frac{\partial Rp}{\partial R} ∂R∂Rp只是非正式的定义方式。
左右扰动的原理: 对 R R R进行一次扰动 Δ R \Delta R ΔR,看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边,最后结果会有一点微小的差异。
左扰动计算:
设左扰动 Δ R \Delta R ΔR对应的李代数为 φ \varphi φ,我们可以计算:
∂ ( R p ) ∂ φ . \frac{\partial(Rp)}{\partial \varphi}. ∂φ∂(Rp).
则:
∂ R p ∂ R = ∂ ( R p ) ∂ φ = lim φ → 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ 泰 勒 展 开 ≈ lim φ → 0 ( I + φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 φ ∧ exp ( ϕ ∧ ) p φ = lim φ → 0 φ ∧ R p φ 根 据 表 1 中 提 供 的 性 质 可 得 = lim φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ \begin{aligned} \frac{\partial Rp}{\partial R} &= \frac{\partial(Rp)}{\partial \varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{\exp(\varphi^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\ 泰勒展开&\approx \lim\limits_{\varphi \to 0} \frac{(I+\varphi^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{\varphi^{\wedge}\exp(\phi^{\wedge})p}{\varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{\varphi^{\wedge}Rp}{\varphi} \\ 根据表1中提供的性质可得 &= \lim\limits_{\varphi \to 0} \frac{-(Rp)^{\wedge}\varphi}{\varphi} \\ &= -(Rp)^{\wedge} \end{aligned} ∂R∂Rp泰勒展开根据表1中提供的性质可得=∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧
右扰动计算:
同理,设右扰动 Δ R \Delta R ΔR对应的李代数为 φ \varphi φ,我们可以计算:
∂ ( R p ) ∂ φ . \frac{\partial(Rp)}{\partial \varphi}. ∂φ∂(Rp).
则:
∂ R p ∂ R = ∂ ( R p ) ∂ φ = lim φ → 0 exp ( ϕ ∧ ) exp ( φ ∧ ) p − exp ( ϕ ∧ ) p φ 泰 勒 展 开 ≈ lim φ → 0 exp ( ϕ ∧ ) ( I + φ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 exp ( ϕ ∧ ) φ ∧ p φ = lim φ → 0 R φ ∧ p φ = lim φ → 0 R φ ∧ R R T p φ 根 据 表 1 中 提 供 的 性 质 ( C u ) ∧ = C u ∧ C T 可 得 : = lim φ → 0 ( R φ ) ∧ ( R T p ) φ 根 据 表 1 中 提 供 的 性 质 u ∧ v = − v ∧ u 可 得 : = lim φ → 0 − ( R T p ) ∧ R φ φ = − ( R T p ) ∧ R \begin{aligned} \frac{\partial Rp}{\partial R} &= \frac{\partial(Rp)}{\partial \varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{\exp(\phi^{\wedge})\exp(\varphi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\ 泰勒展开&\approx \lim\limits_{\varphi \to 0} \frac{\exp(\phi^{\wedge})(I+\varphi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{\exp(\phi^{\wedge})\varphi^{\wedge}p}{\varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{R\varphi^{\wedge}p}{\varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{R\varphi^{\wedge}RR^Tp}{\varphi} \\ 根据表1中提供的性质(Cu)^{\wedge}=Cu^{\wedge}C^T可得 : \\ &= \lim\limits_{\varphi \to 0} \frac{(R\varphi)^{\wedge}(R^Tp)}{\varphi} \\ 根据表1中提供的性质u^{\wedge}v = -v^{\wedge}u可得:\\ &= \lim\limits_{\varphi \to 0} \frac{-(R^Tp)^{\wedge}R\varphi}{\varphi} \\ &=-(R^Tp)^{\wedge}R \end{aligned} ∂R∂Rp泰勒展开根据表1中提供的性质(Cu)∧=Cu∧CT可得:根据表1中提供的性质u∧v=−v∧u可得:=∂φ∂(Rp)=φ→0limφexp(ϕ∧)exp(φ∧)p−exp(ϕ∧)p≈φ→0limφexp(ϕ∧)(I+φ∧)p−exp(ϕ∧)p=φ→0limφexp(ϕ∧)φ∧p=φ→0limφRφ∧p=φ→0limφRφ∧RRTp=φ→0limφ(Rφ)∧(RTp)=φ→0limφ−(RTp)∧Rφ=−(RTp)∧R
旋转的复合。请分别通过左、右扰动的方式,计算:
∂ ln ( R 1 R 2 ) ∨ ∂ R 1 . 和 ∂ ln ( R 1 R 2 ) ∨ ∂ R 2 . \frac{\partial \ln(R_1R_2)^{\vee}}{\partial R_1}. \\ 和\\ \frac{\partial \ln(R_1R_2)^{\vee}}{\partial R_2}. \\ ∂R1∂ln(R1R2)∨.和∂R2∂ln(R1R2)∨.
会使用链式法则,但是这里不太懂为什么需要将( R 1 R 2 R_1R_2 R1R2)转换成向量形式,也不知道如何处理下尖尖符号。所以下边的求导过程可能错漏百出,还望见谅。经过群里求助大佬,我又重新去看了下BHC公式,BHC公式告诉我们,当处理两个矩阵指数之积时,会产生一些由李括号组成的余项。
左扰动:
BHC线性近似表达:
ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 当 ϕ 1 为 小 量 时 J l ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 当 ϕ 2 为 小 量 时 \ln(\exp(\phi_1^{\wedge})\exp(\phi_2^{\wedge}))^{\vee} \approx \begin{cases} \mathcal{J}_l(\phi_2)^{-1}\phi_1 + \phi_2 &当\phi_1为小量时\\ \mathcal{J}_l(\phi_1)^{-1}\phi_2 + \phi_1 &当\phi_2为小量时 \end{cases} ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{
Jl(ϕ2)−1ϕ1+ϕ2Jl(ϕ1)−1ϕ2+ϕ1当ϕ1为小量时当ϕ2为小量时
左乘BHC近似雅克比 J l \mathcal{J}_l Jl:
J l = J = sin θ θ I + ( 1 − sin θ θ ) a a T + ( 1 − cos θ ) θ a ∧ . \mathcal{J}_l = \mathcal{J} = \frac{\sin\theta}{\theta}I + (1-\frac{\sin\theta}{\theta})aa^T + \frac{(1-\cos\theta)}{\theta}a^{\wedge}. Jl=J=θsinθI+(1−θsinθ)aaT+θ(1−cosθ)a∧.
它的逆:
J l − 1 = θ 2 cot θ 2 I + ( 1 − θ 2 cot θ 2 ) a a T − θ 2 a ∧ . \mathcal{J}_l^{-1} = \frac{\theta}{2}\cot\frac{\theta}{2}I + (1-\frac{\theta}{2}\cot\frac{\theta}{2})aa^T - \frac{\theta}{2}a^{\wedge}. Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧.
通过左扰动的方式计算$\frac{\partial \ln(R_1R_2)^{\vee}}{\partial R_1}. $
∂ ln ( R 1 R 2 ) ∨ ∂ R 1 = lim φ 1 → 0 ln ( exp ( φ 1 ∧ ) R 1 R 2 ) ∨ − ln ( R 1 R 2 ) ∨ φ 1 B H C 近 似 ≈ J l ( ln ( R 1 R 2 ) ∨ ) − 1 φ 1 + ln ( R 1 R 2 ) ∨ − ln ( R 1 R 2 ) ∨ φ 1 = J l ( ln ( R 1 R 2 ) ∨ ) − 1 \begin{aligned} \frac{\partial \ln(R_1R_2)^{\vee}}{\partial R_1} &= \lim\limits_{\varphi_1 \to 0} \frac{\ln(\exp(\varphi_1^{\wedge})R_1R_2)^{\vee}-\ln(R_1R_2)^{\vee}}{\varphi_1} \\ BHC近似 &\approx \frac{\mathcal{J_l}(\ln(R_1R_2)^{\vee})^{-1}\varphi_1 + \ln(R_1R_2)^{\vee}- \ln(R_1R_2)^{\vee}}{\varphi_1} \\ &= \mathcal{J_l}(\ln(R_1R_2)^{\vee})^{-1} \end{aligned} ∂R1∂ln(R1R2)∨BHC近似=φ1→0limφ1ln(exp(φ1∧)R1R2)∨−ln(R1R2)∨≈φ1Jl(ln(R1R2)∨)−1φ1+ln(R1R2)∨−ln(R1R2)∨=Jl(ln(R1R2)∨)−1
好像没有用到链式法则,尽力了,但是写不出来。
通过右扰动的方式计算 ∂ ln ( R 1 R 2 ) ∨ ∂ R 2 . \frac{\partial \ln(R_1R_2)^{\vee}}{\partial R_2}. ∂R2∂ln(R1R2)∨.
不会写
事实上, T W C T_{WC} TWC的平移部分即构成了机器人的轨迹。它的物理意义是什么?为何画出 T W C T_{WC} TWC的平移
部分就得到了机器人的轨迹?
首先需要再回顾一下刚体运动的定义,两个坐标系之间的运动由一个旋转加上一个平移组成,这种运动称为刚体运动(课本p43)。而在刚体运动过程中,同一个向量在各个坐标系下的长度和夹角是不发生变化的。
从物理的角度来理解向量这一概念,向量表示一种运动过程。假设摄像机就处于世界坐标系的原点位置(这样好理解一些,其实出于任意位置都可),用 T W C T_{WC} TWC表示从相机坐标系到世界坐标系的转换,转换矩阵中包括旋转部分和平移部分。其中旋转部分可以将两个坐标系的x、y、z对齐,平移部分就表示相机的运动轨迹。所以画出 T W C T_{WC} TWC的平移部分就得到了机器人的运动轨迹。思考了很久,最终只能理解到这个程度,也不知道对不对。
完成数据读取部分代码,让程序run起来。
//主要代码
ifstream fin(trajectory_file);
while (!fin.eof()) {
double time, tx, ty, tz, qx, qy, qz, qw;
fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
poses.push_back(p1);
}
CMakeLists部分代码可见代码文件。
注:由于Sophus版本不一样,需要将提供的代码中SE3改成SE3d。另外需要在代码中添加unistd.h头文件,否则在编译时会出现unsleep的错误提示。
代码运行结果:
本题的代码参考了课本中的示例程序,详细代码和CMakeLists代码可见代码文件。
计算RMSE部分的代码:
double rmse = 0;
//for循环求所有位姿李代数的均方根误差
for (size_t i = 0; i < estimated.size(); i++) {
Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
double error = (p2.inverse() * p1).log().norm();
rmse += error * error;
}
rmse = rmse / double(estimated.size());
rmse = sqrt(rmse);
cout << "RMSE = " << rmse << endl;
轨迹的描绘参考上一题的过程,基本上类似。
得到的误差结果:
最终效果图如下:
github地址: https://github.com/ximing1998/slam-learning.git
国内gitee地址:https://gitee.com/ximing689/slam-learning.git
文章浏览阅读1.6k次。安装配置gi、安装数据库软件、dbca建库见下:http://blog.csdn.net/kadwf123/article/details/784299611、检查集群节点及状态:[root@rac2 ~]# olsnodes -srac1 Activerac2 Activerac3 Activerac4 Active[root@rac2 ~]_12c查看crs状态
文章浏览阅读1.3w次,点赞45次,收藏99次。我个人用的是anaconda3的一个python集成环境,自带jupyter notebook,但在我打开jupyter notebook界面后,却找不到对应的虚拟环境,原来是jupyter notebook只是通用于下载anaconda时自带的环境,其他环境要想使用必须手动下载一些库:1.首先进入到自己创建的虚拟环境(pytorch是虚拟环境的名字)activate pytorch2.在该环境下下载这个库conda install ipykernelconda install nb__jupyter没有pytorch环境
文章浏览阅读5.2k次,点赞19次,收藏28次。选择scoop纯属意外,也是无奈,因为电脑用户被锁了管理员权限,所有exe安装程序都无法安装,只可以用绿色软件,最后被我发现scoop,省去了到处下载XXX绿色版的烦恼,当然scoop里需要管理员权限的软件也跟我无缘了(譬如everything)。推荐添加dorado这个bucket镜像,里面很多中文软件,但是部分国外的软件下载地址在github,可能无法下载。以上两个是官方bucket的国内镜像,所有软件建议优先从这里下载。上面可以看到很多bucket以及软件数。如果官网登陆不了可以试一下以下方式。_scoop-cn
文章浏览阅读4.5k次,点赞2次,收藏3次。首先要有一个color-picker组件 <el-color-picker v-model="headcolor"></el-color-picker>在data里面data() { return {headcolor: ’ #278add ’ //这里可以选择一个默认的颜色} }然后在你想要改变颜色的地方用v-bind绑定就好了,例如:这里的:sty..._vue el-color-picker
文章浏览阅读640次。基于芯片日益增长的问题,所以内核开发者们引入了新的方法,就是在内核中只保留函数,而数据则不包含,由用户(应用程序员)自己把数据按照规定的格式编写,并放在约定的地方,为了不占用过多的内存,还要求数据以根精简的方式编写。boot启动时,传参给内核,告诉内核设备树文件和kernel的位置,内核启动时根据地址去找到设备树文件,再利用专用的编译器去反编译dtb文件,将dtb还原成数据结构,以供驱动的函数去调用。firmware是三星的一个固件的设备信息,因为找不到固件,所以内核启动不成功。_exynos 4412 刷机
文章浏览阅读2w次,点赞24次,收藏42次。Linux系统配置jdkLinux学习教程,Linux入门教程(超详细)_linux配置jdk
文章浏览阅读3.3k次,点赞5次,收藏19次。xlabel('\delta');ylabel('AUC');具体符号的对照表参照下图:_matlab微米怎么输入
文章浏览阅读119次。顺序读写指的是按照文件中数据的顺序进行读取或写入。对于文本文件,可以使用fgets、fputs、fscanf、fprintf等函数进行顺序读写。在C语言中,对文件的操作通常涉及文件的打开、读写以及关闭。文件的打开使用fopen函数,而关闭则使用fclose函数。在C语言中,可以使用fread和fwrite函数进行二进制读写。 Biaoge 于2024-03-09 23:51发布 阅读量:7 ️文章类型:【 C语言程序设计 】在C语言中,用于打开文件的函数是____,用于关闭文件的函数是____。
文章浏览阅读3.4k次,点赞2次,收藏13次。跟随鼠标移动的粒子以grid(SOP)为partical(SOP)的资源模板,调整后连接【Geo组合+point spirit(MAT)】,在连接【feedback组合】适当调整。影响粒子动态的节点【metaball(SOP)+force(SOP)】添加mouse in(CHOP)鼠标位置到metaball的坐标,实现鼠标影响。..._touchdesigner怎么让一个模型跟着鼠标移动
文章浏览阅读178次。项目运行环境配置:Jdk1.8 + Tomcat7.0 + Mysql + HBuilderX(Webstorm也行)+ Eclispe(IntelliJ IDEA,Eclispe,MyEclispe,Sts都支持)。项目技术:Springboot + mybatis + Maven +mysql5.7或8.0+html+css+js等等组成,B/S模式 + Maven管理等等。环境需要1.运行环境:最好是java jdk 1.8,我们在这个平台上运行的。其他版本理论上也可以。_基于java技术的停车场管理系统实现与设计
文章浏览阅读3.5k次。前言对于MediaPlayer播放器的源码分析内容相对来说比较多,会从Java-&amp;gt;Jni-&amp;gt;C/C++慢慢分析,后面会慢慢更新。另外,博客只作为自己学习记录的一种方式,对于其他的不过多的评论。MediaPlayerDemopublic class MainActivity extends AppCompatActivity implements SurfaceHolder.Cal..._android多媒体播放源码分析 时序图
文章浏览阅读2.4k次,点赞41次,收藏13次。java 数据结构与算法 ——快速排序法_快速排序法