SLAM 14 notes
4.23 推导
f ( x ) f(x) f(x)在点a处的泰勒展开
f ( x ) = ∑ n = 0 ∞ f ( n ) a n ! ( x − a ) n f(x) = \sum_{n=0}^\infty \frac{f^{(n)}a}{n!}(x-a)^n f(x)=∑n=0∞n!f(n)a(x−a)n
l n x lnx lnx的n阶导数
l n ( n ) x = ( − 1 ) n − 1 ( n − 1 ) ! x n ln^{(n)}x = \frac{(-1)^{n-1}(n-1)!}{x^n} ln(n)x=xn(−1)n−1(n−1)!
l n ( R ) ln(R) ln(R)在 I I I处展开
l n ( R ) = ∑ n = 0 ∞ l n ( n ) I ( R − I ) n n ! = ∑ n = 1 ∞ l n ( n ) I ( R − I ) n n ! = ∑ n = 1 ∞ ( − 1 ) n − 1 ( R − I ) n n = ∑ n = 0 ∞ ( − 1 ) n ( R − I ) n + 1 n + 1 \begin{aligned} ln(R) & =\sum_{n=0}^\infty\frac{ln^{(n)}I(R-I)^n}{n!} \\ & = \sum_{n=1}^\infty\frac{ln^{(n)}I(R-I)^n}{n!} \\ & = \sum_{n=1}^\infty\frac{(-1)^{n-1}(R-I)^n}{n}\\ & = \sum_{n=0}^\infty\frac{(-1)^{n}(R-I)^{n+1}}{n+1} \end{aligned} ln(R)=n=0∑∞n!ln(n)I(R−I)n=n=1∑∞n!ln(n)I(R−I)n=n=1∑∞n(−1)n−1(R−I)n=n=0∑∞n+1(−1)n(R−I)n+1
4.24 推导
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] \begin{aligned} \xi ^ \wedge = \begin{bmatrix} \phi ^ \wedge & \rho\\ 0^T & 0 \end{bmatrix} \end{aligned} ξ∧=[ϕ∧0Tρ0]
e x p ( ξ ∧ ) = ∑ n = 0 ∞ 1 n ! [ ϕ ∧ ρ 0 T 0 ] n = I + ∑ n = 1 ∞ 1 n ! [ ϕ ∧ ρ 0 T 0 ] n = [ I 0 0 T 1 ] + ∑ n = 1 ∞ 1 n ! [ ( ϕ ∧ ) n ( ϕ ∧ ) n − 1 ρ 0 T 0 ] = [ I + ∑ n = 1 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 1 ∞ 1 n ! ( ϕ ∧ ) n − 1 ρ 0 T 1 ] = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] \begin{aligned} 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 = \begin{bmatrix}I & 0\\ 0^T & 1 \end{bmatrix} + \sum_{n=1}^{\infty}\frac{1}{n!}\begin{bmatrix} (\phi ^ \wedge)^n & (\phi ^ \wedge)^{n-1}\rho\\ 0^T & 0 \end{bmatrix} \\ &=\begin{bmatrix}I + \sum_{n=1}^{\infty}\frac{1}{n!}(\phi ^ \wedge)^n & \sum_{n=1}^{\infty}\frac{1}{n!}(\phi ^ \wedge)^{n-1}\rho\\ 0^T & 1 \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} exp(ξ∧)=n=0∑∞n!1[ϕ∧0Tρ0]n=I+n=1∑∞n!1[ϕ∧0Tρ0]n=[I0T01]+n=1∑∞n!1[(ϕ∧)n0T(ϕ∧)n−1ρ0]=[I+∑n=1∞n!1(ϕ∧)n0T∑n=1∞n!1(ϕ∧)n−1ρ1]=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]
4.48 SE3的伴随性质的证明
先算右边
A d ( T ) ξ = [ R t ∧ R 0 T R ] [ ρ ϕ ] = [ R ρ + t ∧ R ϕ R ϕ ] \begin{aligned} Ad(T)\xi = \begin{bmatrix}R & t ^ \wedge R\\ 0^T & R \end{bmatrix} \begin{bmatrix} \rho \\ \phi\end{bmatrix}=\begin{bmatrix} R\rho + t ^ \wedge R\phi\\ R\phi\end{bmatrix} \end{aligned} Ad(T)ξ=[R0Tt∧RR][ρϕ]=[Rρ+t∧RϕRϕ]
再算左边
T e x p ( ξ ∧ ) T − 1 = [ R t 0 T 1 ] [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] [ R − 1 − R − 1 t 0 T 1 ] = [ R ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n R − 1 − R ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n R − 1 t + R ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ + t 0 T 1 ] = [ ∑ n = 0 ∞ R ( ϕ ∧ ) n R − 1 n ! − ∑ n = 0 ∞ R ( ϕ ∧ ) n R − 1 n ! t + R ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n R − 1 R ρ + t 0 T 1 ] = [ ∑ n = 0 ∞ R ( ϕ ∧ ) n R − 1 n ! − ∑ n = 0 ∞ R ( ϕ ∧ ) n R − 1 n ! t + ∑ n = 0 ∞ R ( ϕ ∧ ) n R − 1 ( n + 1 ) ! R ρ + t 0 T 1 ] = [ ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n n ! − ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n n ! t + ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n ( n + 1 ) ! R ρ + t 0 T 1 ] / / 上面这步用到了 R ( ϕ ∧ ) n R − 1 = ( ( R ϕ ) ∧ ) n , 后面我们证明这个等式 = [ ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n n ! − t − ∑ n = 1 ∞ ( ( R ϕ ) ∧ ) n n ! t + ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n ( n + 1 ) ! R ρ + t 0 T 1 ] = [ ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n n ! − ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n + 1 ( n + 1 ) ! t + ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n ( n + 1 ) ! R ρ 0 T 1 ] = [ ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n n ! ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n ( n + 1 ) ! ( R ρ − ( R ϕ ) ∧ t ) 0 T 1 ] = [ ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n n ! ∑ n = 0 ∞ ( ( R ϕ ) ∧ ) n ( n + 1 ) ! ( R ρ + t ∧ ( R ϕ ) ) 0 T 1 ] = e x p ( [ R ρ + t ∧ R ϕ R ϕ ] ) \begin{aligned} &Texp(\xi^ \wedge)T^{-1}=\begin{bmatrix}R & t \\ 0^T & 1 \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}\begin{bmatrix}R^{-1} & -R^{-1}t \\ 0^T & 1 \end{bmatrix} \\ &=\begin{bmatrix}R\sum_{n=0}^{\infty}\frac{1}{n!}(\phi ^ \wedge)^nR^{-1} & -R\sum_{n=0}^{\infty}\frac{1}{n!}(\phi ^ \wedge)^nR^{-1}t + R\sum_{n=0}^{\infty}\frac{1}{(n+1)!}(\phi ^ \wedge)^{n}\rho+t\\ 0^T & 1 \end{bmatrix}\\ &=\begin{bmatrix}\sum_{n=0}^{\infty}\frac{R(\phi ^ \wedge)^nR^{-1}}{n!} & -\sum_{n=0}^{\infty}\frac{R(\phi ^ \wedge)^nR^{-1}}{n!}t + R\sum_{n=0}^{\infty}\frac{1}{(n+1)!}(\phi ^ \wedge)^{n}R^{-1}R\rho+t\\ 0^T & 1 \end{bmatrix}\\ &=\begin{bmatrix}\sum_{n=0}^{\infty}\frac{R(\phi ^ \wedge)^nR^{-1}}{n!} & -\sum_{n=0}^{\infty}\frac{R(\phi ^ \wedge)^nR^{-1}}{n!}t + \sum_{n=0}^{\infty}\frac{R(\phi ^ \wedge)^{n}R^{-1}}{(n+1)!}R\rho+t\\ 0^T & 1 \end{bmatrix}\\ &=\begin{bmatrix}\sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^n}{n!} & -\sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^n}{n!}t + \sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^{n}}{(n+1)!}R\rho+t\\ 0^T & 1 \end{bmatrix}\\ &//上面这步用到了R(\phi ^ \wedge)^nR^{-1} = ((R\phi) ^ \wedge)^n, 后面我们证明这个等式\\ &=\begin{bmatrix}\sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^n}{n!} & -t - \sum_{n=1}^{\infty}\frac{((R\phi) ^ \wedge)^n}{n!}t + \sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^{n}}{(n+1)!}R\rho+t\\ 0^T & 1 \end{bmatrix}\\ &=\begin{bmatrix}\sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^n}{n!} & - \sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^{n+1}}{(n+1)!}t + \sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^{n}}{(n+1)!}R\rho\\ 0^T & 1 \end{bmatrix}\\ &=\begin{bmatrix}\sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^n}{n!} & \sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^{n}}{(n+1)!}(R\rho-(R\phi) ^ \wedge t)\\ 0^T & 1 \end{bmatrix}\\ &=\begin{bmatrix}\sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^n}{n!} & \sum_{n=0}^{\infty}\frac{((R\phi) ^ \wedge)^{n}}{(n+1)!}(R\rho+t^ \wedge (R\phi) )\\ 0^T & 1 \end{bmatrix}\\ &=exp(\begin{bmatrix} R\rho + t ^ \wedge R\phi\\ R\phi\end{bmatrix}) \end{aligned} Texp(ξ∧)T−1=[R0Tt1][∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1][R−10T−R−1t1]=[R∑n=0∞n!1(ϕ∧)nR−10T−R∑n=0∞n!1(ϕ∧)nR−1t+R∑n=0∞(n+1)!1(ϕ∧)nρ+t1]=[∑n=0∞n!R(ϕ∧)nR−10T−∑n=0∞n!R(ϕ∧)nR−1t+R∑n=0∞(n+1)!1(ϕ∧)nR−1Rρ+t1]=[∑n=0∞n!R(ϕ∧)nR−10T−∑n=0∞n!R(ϕ∧)nR−1t+∑n=0∞(n+1)!R(ϕ∧)nR−1Rρ+t1]=[∑n=0∞n!((Rϕ)∧)n0T−∑n=0∞n!((Rϕ)∧)nt+∑n=0∞(n+1)!((Rϕ)∧)nRρ+t1]//上面这步用到了R(ϕ∧)nR−1=((Rϕ)∧)n,后面我们证明这个等式=[∑n=0∞n!((Rϕ)∧)n0T−t−∑n=1∞n!((Rϕ)∧)nt+∑n=0∞(n+1)!((Rϕ)∧)nRρ+t1]=[∑n=0∞n!((Rϕ)∧)n0T−∑n=0∞(n+1)!((Rϕ)∧)n+1t+∑n=0∞(n+1)!((Rϕ)∧)nRρ1]=[∑n=0∞n!((Rϕ)∧)n0T∑n=0∞(n+1)!((Rϕ)∧)n(Rρ−(Rϕ)∧t)1]=[∑n=0∞n!((Rϕ)∧)n0T∑n=0∞(n+1)!((Rϕ)∧)n(Rρ+t∧(Rϕ))1]=exp([Rρ+t∧RϕRϕ])
得证. 下面证明 R ( ϕ ∧ ) n R − 1 = ( ( R ϕ ) ∧ ) n R(\phi ^ \wedge)^nR^{-1} = ((R\phi) ^ \wedge)^n R(ϕ∧)nR−1=((Rϕ)∧)n
令 a = b ∧ c R a = R ( b ∧ c ) R a = ( R b ) ∧ ( R c ) a = R − 1 ( R b ) ∧ ( R c ) a = R − 1 ( R b ) ∧ R c 则 b ∧ = R − 1 ( R b ) ∧ R R b ∧ R − 1 = ( R b ) ∧ 同理 令 a = ( b ∧ ) n c R a = ( ( R b ) ∧ ) n R c a = R − 1 ( ( R b ) ∧ ) n R c 则 ( b ∧ ) n = R − 1 ( ( R b ) ∧ ) n R R ( b ∧ ) n R − 1 = ( ( R b ) ∧ ) n \begin{aligned} 令a &= b^ \wedge c \\ Ra &= R(b^ \wedge c) \\ Ra &= (Rb)^ \wedge (Rc)\\ a &= R^{-1}(Rb)^ \wedge (Rc)\\ a &= R^{-1}(Rb)^ \wedge Rc\\ 则 b ^ \wedge&= R^{-1}(Rb)^ \wedge R\\ Rb ^ \wedge R^{-1} &= (Rb)^ \wedge\\ 同理\\ 令a &= (b^ \wedge)^n c \\ Ra &= ((Rb)^ \wedge)^n Rc\\ a &= R^{-1} ((Rb)^ \wedge)^n Rc\\ 则 (b^ \wedge)^n &= R^{-1} ((Rb)^ \wedge)^n R\\ R(b^ \wedge)^n R^{-1} &= ((Rb)^ \wedge)^n \end{aligned} 令aRaRaaa则b∧Rb∧R−1同理令aRaa则(b∧)nR(b∧)nR−1=b∧c=R(b∧c)=(Rb)∧(Rc)=R−1(Rb)∧(Rc)=R−1(Rb)∧Rc=R−1(Rb)∧R=(Rb)∧=(b∧)nc=((Rb)∧)nRc=R−1((Rb)∧)nRc=R−1((Rb)∧)nR=((Rb)∧)n