SLAM 14 notes

4.23 推导

SLAM 14 notes_第1张图片

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=0n!f(n)a(xa)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)n1(n1)!

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=0n!ln(n)I(RI)n=n=1n!ln(n)I(RI)n=n=1n(1)n1(RI)n=n=0n+1(1)n(RI)n+1

4.24 推导

SLAM 14 notes_第2张图片

ξ ∧ = [ ϕ ∧ ρ 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=0n!1[ϕ0Tρ0]n=I+n=1n!1[ϕ0Tρ0]n=[I0T01]+n=1n!1[(ϕ)n0T(ϕ)n1ρ0]=[I+n=1n!1(ϕ)n0Tn=1n!1(ϕ)n1ρ1]=[n=0n!1(ϕ)n0Tn=0(n+1)!1(ϕ)nρ1]

4.48 SE3的伴随性质的证明

SLAM 14 notes_第3张图片

先算右边

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)ξ=[R0TtRR][ρϕ]=[+t]

再算左边
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(ξ)T1=[R0Tt1][n=0n!1(ϕ)n0Tn=0(n+1)!1(ϕ)nρ1][R10TR1t1]=[Rn=0n!1(ϕ)nR10TRn=0n!1(ϕ)nR1t+Rn=0(n+1)!1(ϕ)nρ+t1]=[n=0n!R(ϕ)nR10Tn=0n!R(ϕ)nR1t+Rn=0(n+1)!1(ϕ)nR1+t1]=[n=0n!R(ϕ)nR10Tn=0n!R(ϕ)nR1t+n=0(n+1)!R(ϕ)nR1+t1]=[n=0n!(())n0Tn=0n!(())nt+n=0(n+1)!(())n+t1]//上面这步用到了R(ϕ)nR1=(())n,后面我们证明这个等式=[n=0n!(())n0Ttn=1n!(())nt+n=0(n+1)!(())n+t1]=[n=0n!(())n0Tn=0(n+1)!(())n+1t+n=0(n+1)!(())n1]=[n=0n!(())n0Tn=0(n+1)!(())n(()t)1]=[n=0n!(())n0Tn=0(n+1)!(())n(+t())1]=exp([+t])

得证. 下面证明 R ( ϕ ∧ ) n R − 1 = ( ( R ϕ ) ∧ ) n R(\phi ^ \wedge)^nR^{-1} = ((R\phi) ^ \wedge)^n R(ϕ)nR1=(())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} aRaRaaabRbR1同理aRaa(b)nR(b)nR1=bc=R(bc)=(Rb)(Rc)=R1(Rb)(Rc)=R1(Rb)Rc=R1(Rb)R=(Rb)=(b)nc=((Rb))nRc=R1((Rb))nRc=R1((Rb))nR=((Rb))n

你可能感兴趣的:(概率论,机器学习,人工智能)