SLAM从入门到放弃：SLAM十四讲第四章习题（1-4）

以下均为简单笔记，如有错误，请多多指教。

1. 验证$SO(3)$、$SE(3)$和$Sim(3)$关于乘法成群。
   证：
   已知$SO(3)=\{R\in {\mathbb{R}}^{3\times 3}|R{R}^T=I,det(R)=1\}$;取$R_1\in SO(3)$，$R_2\in SO(3)$，$R_3\in SO(3)$;
   验证封闭性：设$R=R_1{R}_2$；不难发现$R{R}^T=(R_1{R}_2)(R_1{R}_2{)}^T=R_1{R}_2{R}_2^T{R}_1^T=I$；并且$det(R)=det(R_1)det(R_2)=1$；故$R\in SO(3)$；
   验证结合律：根据矩阵乘法结合律，不难得到$(R_1{R}_2)R_3=R_1(R_2{R}_3)$；
   幺元：根据单位矩阵的特性，可以发现$IR=RI=R$；
   逆：由于单位旋转矩阵具有正定性，即$R{R}^T=I$。
   –
   已知$SE(3)=\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\}$;
   验证封闭性：设$T=T_1{T}_2$，故$T=\left[\begin{array}{cc}{R}_1{R}_2& R_1{t}_2+t_1\\ 0^T& 1\end{array}\right]$，由于$R_1{R}_2\in SO(3)$，且$R_1{t}_2+t_1\in R^3$，因此$T\in SE(3)$；
   验证结合律：根据矩阵乘法结合律，不难得到$(T_1{T}_2)T_3=T_1(T_2{T}_3)$；
   幺元：根据单位矩阵的特性，可以发现$IT=TI=T$；
   逆：对于$T$，必然存在$T_{i}nv=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$使得$T{T}_{i}nv=I$。
   由于$SE(3)$与$SO(3)$并无明显区别，此处不再验证。
   

2. 验证$({\mathbb{R}}^3,\mathbb{R},\times )$构成李代数。
   证：
   假设$X,Y,Z\in {\mathbb{R}}^3$，
   封闭性：不难发现$[X,Y]=X^{\wedge }Y\in {\mathbb{R}}^3$；
   双线性：$[aX+bY,Z]=(aX+bY)Z=a{X}^{\wedge }Z+b{Y}^{\wedge }Z=a[X,Z]+b[Y,Z]$；同理$[Z，aX+bY]=Z(aX+bZ)=a{Z}^{\wedge }X+b{Z}^{\wedge }Y=a[Z,X]+b[Z,Y]$；
   自反性：根据向量运行，明显$[X,X]=X^{\wedge }X=0$;
   雅克比等价：由于$[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=[X,Y^{\wedge }Z]+[Z,X^{\wedge }Y]+[Y,Z^{\wedge }X]=X^{\wedge }(Y^{\wedge }Z)+Z^{\wedge }(X^{\wedge }Y)+Y^{\wedge }(Z^{\wedge }X)$
   根据三重积展开公式可以得到
   $X^{\wedge }(Y^{\wedge }Z)=Y(X\bullet Z)-Z(X\bullet Y)$
   $Z^{\wedge }(X^{\wedge }Y)=X(Z\bullet Y)-Y(Z\bullet X)$
   $Y^{\wedge }(Z^{\wedge }X)=Z(Y\bullet Z)-X(Y\bullet Z)$
   上式相加为$0$，得证。
   

3. 证$so(3)$和$se(3)$满足李代数要求的性质。 验证$so(3)$和$se(3)$满足李代数要求的性质。
   证：
   $so(3)$的李括号计算为$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-\Phi 2\Phi 1{)}^{\vee }$，其中$\Phi$对应的是$\varphi$的反对称矩阵，${\varphi }_1=(a,b,c{)}^T$，${\varphi }_2=(d,e,f{)}^T$
   封闭性：$({\Phi }_1{\Phi }_2-\Phi 2\Phi 1)=\left[\begin{array}{ccc}0& -c& b\\ c& 0& -a\\ -b& a& 0\end{array}\right]\left[\begin{array}{ccc}0& -f& e\\ f& 0& -d\\ -e& d& 0\end{array}\right]-\left[\begin{array}{ccc}0& -f& e\\ f& 0& -d\\ -e& d& 0\end{array}\right]\left[\begin{array}{ccc}0& -c& b\\ c& 0& -a\\ -b& a& 0\end{array}\right]=\left[\begin{array}{ccc}-cf-be& bd& dc\\ ae& -cf-ad& ce\\ af& bf& -be-ad\end{array}\right]-\left[\begin{array}{ccc}-cf-be& ae& af\\ bd& -cf-ad& bf\\ dc& ce& -be-ad\end{array}\right]=\left[\begin{array}{ccc}0& bd-ae& dc-af\\ ae-bd& -0& ce-bf\\ af-dc& bf-ce& 0\end{array}\right]$，可以发现李括号运算后仍旧是对称矩形，即可以通过$\vee$将其转换为三维向量；
   双线性：$[a{\varphi }_1+b{\varphi }_2,{\varphi }_3]=((a{\varphi }_1+b{\varphi }_2{)}^{\wedge }{\Phi }_3-{\Phi }_3(a{\varphi }_1+b{\varphi }_2{)}^{\wedge }{)}^{\vee }$；$\wedge$运算明显满足$({\varphi }_1+{\varphi }_2{)}^{\wedge }={\varphi }_1^{\wedge }+{\varphi }_2^{\wedge }$；因此上式可以整理得到$((a{\varphi }_1+b{\varphi }_2{)}^{\wedge }{\Phi }_3-{\Phi }_3(a{\varphi }_1+b{\varphi }_2{)}^{\wedge }{)}^{\vee }=(a{\Phi }_1{\Phi }_3-a{\Phi }_3{\Phi }_1+b{\Phi }_2{\Phi }_3-b{\Phi }_3\Phi 2{)}^{\vee }=a[{\varphi }_1,{\varphi }_3]+b[{\varphi }_2,{\varphi }_3]$，得证；
   自反性：明显$[\varphi ,\varphi ]=(\Phi \Phi -\Phi \Phi {)}^{\vee }=0$；
   雅克比等价：$[{\varphi }_1,[{\varphi }_2,{\varphi }_3]]=[{\varphi }_1,({\Phi }_2{\Phi }_3-{\Phi }_3{\Phi }_2{)}^{\vee }]=({\Phi }_1({\Phi }_2{\Phi }_3-{\Phi }_3{\Phi }_2)-({\Phi }_2{\Phi }_3-{\Phi }_3{\Phi }_2){\Phi }_1{)}^{\vee }=({\Phi }_1{\Phi }_2{\Phi }_3-{\Phi }_1{\Phi }_3{\Phi }_2-{\Phi }_2{\Phi }_3{\Phi }_1+{\Phi }_3{\Phi }_2{\Phi }_1{)}^{\vee }$，
   $[{\varphi }_3,[{\varphi }_1,{\varphi }_2]]=({\Phi }_3{\Phi }_1{\Phi }_2-{\Phi }_3{\Phi }_2{\Phi }_1-{\Phi }_1{\Phi }_2{\Phi }_3+{\Phi }_2{\Phi }_1{\Phi }_3{)}^{\vee }$
   $[{\varphi }_2,[{\varphi }_3,{\varphi }_1]]=({\Phi }_2{\Phi }_3{\Phi }_1-{\Phi }_2{\Phi }_1{\Phi }_3-{\Phi }_3{\Phi }_1{\Phi }_2+{\Phi }_1{\Phi }_3{\Phi }_2{)}^{\vee }$
   故$[{\varphi }_1,[{\varphi }_2,{\varphi }_3]]+[{\varphi }_3,[{\varphi }_1,{\varphi }_2]]+[{\varphi }_2,[{\varphi }_3,{\varphi }_1]]=({\Phi }_1{\Phi }_2{\Phi }_3-{\Phi }_1{\Phi }_3{\Phi }_2-{\Phi }_2{\Phi }_3{\Phi }_1+{\Phi }_3{\Phi }_2{\Phi }_1+{\Phi }_3{\Phi }_1{\Phi }_2-{\Phi }_3{\Phi }_2{\Phi }_1-{\Phi }_1{\Phi }_2{\Phi }_3+{\Phi }_2{\Phi }_1{\Phi }_3+{\Phi }_2{\Phi }_3{\Phi }_1-{\Phi }_2{\Phi }_1{\Phi }_3-{\Phi }_3{\Phi }_1{\Phi }_2+{\Phi }_1{\Phi }_3{\Phi }_2{)}^{\vee }=0$。
   –
   $se(3)$的李括号计算为$[{\epsilon }_1,{\epsilon }_2]=({\epsilon }_1^{\wedge }{\epsilon }_2^{\wedge }-{\epsilon }_2^{\wedge }{\epsilon }_1^{\wedge }{)}^{\vee }$，其中${\epsilon }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]$;
   封闭性：$[{\epsilon }_1,{\epsilon }_2]=(\left[\begin{array}{cc}{\Phi }_1& {\rho }_1\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_2& {\rho }_2\\ 0^T& 0\end{array}\right]-\left[\begin{array}{cc}{\Phi }_2& {\rho }_2\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_1& {\rho }_1\\ 0^T& 0\end{array}\right]{)}^{\vee }=(\left[\begin{array}{cc}{\Phi }_1{\Phi }_2& {\Phi }_1{\rho }_2\\ 0^T& 0\end{array}\right]-\left[\begin{array}{cc}{\Phi }_2{\Phi }_1& {\Phi }_2{\rho }_1\\ 0^T& 1\end{array}\right]{)}^{\vee }=(\left[\begin{array}{cc}{\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1& {\Phi }_1{\rho }_2-{\Phi }_2{\rho }_1\\ 0^T& 0\end{array}\right]{)}^{\vee }$，可以证明计算后得到的值属于$se(3)$；
   双线性：$[a{\epsilon }_1+b{\epsilon }_2,{\epsilon }_3]=((a{\epsilon }_1+b{\epsilon }_2{)}^{\wedge }{\epsilon }_3^{\wedge }-{\epsilon }_3^{\wedge }(a{\epsilon }_1+b{\epsilon }_2{)}^{\wedge }{)}^{\vee }$，虽然此处的$\wedge$非常复杂，但是明显也存在$({\epsilon }_1+{\epsilon }_2{)}^{\wedge }={\epsilon }_1\wedge +{\epsilon }_2\wedge$(我也是猜想~)，因此上式就可以类似$so(3)$展开证明；
   自反性：$[\epsilon ,\epsilon ]=({\epsilon }^{\wedge }{\epsilon }^{\wedge }-{\epsilon }^{\wedge }{\epsilon }^{\wedge }{)}^{\vee }=0$；
   雅克比等价：$[{\epsilon }_1,[{\epsilon }_2,{\epsilon }_3]]=[{\epsilon }_1,({\epsilon }_2^{\wedge }{\epsilon }_3^{\wedge }-{\epsilon }_3^{\wedge }{\epsilon }_2^{\wedge }{)}^{\vee }]=({\epsilon }_1^{\wedge }({\epsilon }_2^{\wedge }{\epsilon }_3^{\wedge })-({\epsilon }_2^{\wedge }{\epsilon }_3^{\wedge }){\epsilon }_1^{\wedge }{)}^{\vee }$
   $[{\epsilon }_3,[{\epsilon }_1,{\epsilon }_2]]=({\epsilon }_3^{\wedge }({\epsilon }_1^{\wedge }{\epsilon }_2^{\wedge })-({\epsilon }_1^{\wedge }{\epsilon }_2^{\wedge }){\epsilon }_3^{\wedge }{)}^{\vee }$
   $[{\epsilon }_2,[{\epsilon }_3,{\epsilon }_1]]=({\epsilon }_2^{\wedge }({\epsilon }_3^{\wedge }{\epsilon }_1^{\wedge })-({\epsilon }_3^{\wedge }{\epsilon }_1^{\wedge }){\epsilon }_2^{\wedge }{)}^{\vee }$
   三者相加就可证明。
   

4. 证明性质$(4.20)$和$(4.21)$。
   证：
   首先记$a=(x,y,z{)}^T$，且$x^2+y^2+z^2=1$。
   $(4.20)$为$a^{\wedge }{a}^{\wedge }=a{a}^T-I$，，
   则$a^{\wedge }{a}^{\wedge }=\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]=\left[\begin{array}{ccc}-y^2-z^2& xy& xz\\ xy& -z^2-x^2& yz\\ xz& yz& -x^2-y^2\end{array}\right]$
   $a{a}^T-I=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\left[\begin{array}{ccc}x& y& z\end{array}\right]-I=\left[\begin{array}{ccc}{x}^2& xy& xz\\ xy& y^2& yz\\ xz& yz& z^2\end{array}\right]-I=\left[\begin{array}{ccc}{x}^2-1& xy& xz\\ xy& y^2-1& yz\\ xz& yz& z^2-1\end{array}\right]=\left[\begin{array}{ccc}-y^2-z^2& xy& xz\\ xy& -z^2-x^2& yz\\ xz& yz& -x^2-y^2\end{array}\right]$
   故得证。
   –
   $(4.21)$为$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }$，由于$a^{\wedge }{a}^{\wedge }=\left[\begin{array}{ccc}-y^2-z^2& xy& xz\\ xy& -z^2-x^2& yz\\ xz& yz& -x^2-y^2\end{array}\right]$，故$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=\left[\begin{array}{ccc}-y^2-z^2& xy& xz\\ xy& -z^2-x^2& yz\\ xz& yz& -x^2-y^2\end{array}\right]\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]=\left[\begin{array}{ccc}0& z(y^2+z^2)+z{x}^2& -y(y^2+z^2)-y{x}^2\\ z(-z^2-x^2)-z{y}^2& 0& x{y}^2+x(z^2+x^2)\\ y{z}^2+y(x^2+y^2)& -x{z}^2-x(x^2+y^2)& 0\end{array}\right]=\left[\begin{array}{ccc}0& z& -y\\ -z& 0& x\\ y& -x& 0\end{array}\right]=-a^{\wedge }$