第三节课习题

2.群的性质

课上我们讲解了什么是群。请根据群定义，求解以下问题：

1. { Z , + } \{\mathbb{Z}, + \} { Z,+}是否为群？若是，验证其满足群定义；若不是，说明理由。

   根据群定义可知 { Z , + } \{\mathbb{Z}, + \} { Z,+}是群。证明如下：

   封闭性：$\forall z_1,z_2\in \mathbb{Z},z_1+z_2\in \mathbb{Z}.$

   结合律：$\forall z_1,z_2,z_3\in \mathbb{Z},(z_1+z_2)+z_3=z_1+(z_2+z_3).$

   幺元：$\exists 0\in \mathbb{Z},s.t.\forall z\in \mathbb{Z},0+z=z+0=z.$

   逆：$\forall z\in \mathbb{Z},\exists -z\in \mathbb{Z},s.t.z+(-z)=0.$

2. { N , + } \{\mathbb{N}, + \} { N,+}是否为群？若是，验证其满足群定义；若不是，说明理由。

   根据群定义可知 { N , + } \{\mathbb{N}, + \} { N,+}不是群。证明如下：

   封闭性：$\forall n_1,n_2\in \mathbb{N},n_1+n_2\in \mathbb{N}.$

   结合律：$\forall n_1,n_2,n_3\in \mathbb{N},(n_1+n_2)+n_3=n_1+(n_2+n_3).$

   幺元：$\exists 0\in \mathbb{N},s.t.\forall n\in \mathbb{N},0+n=n+0=n.$

   但是不满足逆的性质，对于任意的$n\in \mathbb{N}$无法找到其逆$n^{-1}$，使得$n+n^{-1}=0$。

3. 解释什么是阿贝尔群。并说明矩阵及乘法构成的群是否为阿贝尔群。

   ​ 阿贝尔群（Abel Group）又称交换群或可交换群，它由自身的集合G和二元运算*构成，它除了满足一般的群公理之外，还满足交换律。因为阿贝尔群的群运算满足交换律和结合律，群元素乘积的值与乘法运算时的次序无关。

   ​ 矩阵乘法构成的群不是阿贝尔群，因为矩阵乘法不满足交换律。

3.验证向量叉乘的李代数性质

解题思路，将括号部分带入性质定义。

​ 封闭性：集合中任意两元素通过李括号运算得到的结果仍属于集合，$a\times b$得到的结果仍是一个三维向量。

​ 即 $\forall X,Y\in {\mathbb{R}}^3,有X\times Y\in {\mathbb{R}}^3.$

​ 双线性：对于$\forall X,Y,Z\in {\mathbb{R}}^3,a,b\in \mathbb{R}$，向量叉乘运算满足乘法分配律，因此可得，

​ $[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$满足双线性。

​ 自反性：因为向量和自己叉乘结果为0，集合中元素和自己通过李括号运算结果为0，因此满足自反性。

​ 雅克比等价：对于$\forall X,Y,Z\in {\mathbb{R}}^3,$有

​ $ [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\times (Y\times Z)=Y(X\cdot Z)-Z(X\cdot Y)$代入上式可得：

​ $\begin{array}{rl}[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{array}$

​ 所以满足雅克比等价。

因此 $\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$构成李代数。

4.推导SE(3)的指数映射

推导过程如下：
$$\begin{array}{rl}设\xi & =[\rho ,\varphi {]}^T\in \mathfrak{s}\mathfrak{e}(3),\\ {\xi }^{\wedge }& =\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\\ 其中,& \\ {\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^2& =\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\times \left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\\ & =\left[\begin{array}{cc}({\varphi }^{\wedge }{)}^2& {\varphi }^{\wedge }\rho \\ 0^T& 0\end{array}\right]\\ {\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^3& ={\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^2\times \left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\\ & =\left[\begin{array}{cc}({\varphi }^{\wedge }{)}^3& {\varphi }^{\wedge }{\varphi }^{\wedge }\rho \\ 0^T& 0\end{array}\right]\\ 因此，& \\ {\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^n& ={\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^n\times \left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\\ & =\left[\begin{array}{cc}({\varphi }^{\wedge }{)}^n& ({\varphi }^{\wedge }{)}^{n-1}\rho \\ 0^T& 0\end{array}\right]\\ 将指数映射写成一个泰勒展开可得，& \\ \exp ({\xi }^{\wedge })& =\sum _{n=0}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^n\\ & =I+\sum _{n=1}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]}^n\\ & =I+\left[\begin{array}{cc}{\sum }_{n=1}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& {\sum }_{n=1}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^{n-1}\rho \\ 0^T& 0\end{array}\right]\\ & =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right]\end{array}$$
式（2）得证。
$$\begin{gathered} 令\varphi =\theta a,那么, \\ \begin{array}{rl}\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\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+\left(\begin{array}{c}\frac{1}{2!}\theta -\frac{1}{4!}{\theta }^3+\cdots \end{array}\right)a^{\wedge }+\left(\begin{array}{c}\frac{1}{3!}{\theta }^2-\frac{1}{5!}{\theta }^4+\cdots \end{array}\right)a^{\wedge }{a}^{\wedge }\\ & =I+\frac{1}{\theta }\left(\begin{array}{c}\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+\cdots \end{array}\right)a^{\wedge }+\frac{1}{\theta }\left(\begin{array}{c}\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots \end{array}\right)(a{a}^T-I)\end{array} \end{gathered}$$
根据泰勒展开式：
$$\begin{gathered} \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 \end{gathered}$$
得
$$\begin{gathered} 令\varphi =\theta a,, \\ 其中，\theta 为模长，a为单位长度为1的单位向量 \\ 有,a^{\wedge }{a}^{\wedge }=a{a}^T-I \\ {a}^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge } \\ \begin{array}{rl}\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n& =\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\theta a^{\wedge }{)}^n\\ & =I+\frac{1}{\theta }\left(\begin{array}{c}\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+\cdots \end{array}\right)a^{\wedge }+\frac{1}{\theta }\left(\begin{array}{c}\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots \end{array}\right)(a{a}^T-I)\\ & =I+\frac{1-\cos \theta }{\theta }{a}^{\wedge }+\frac{1}{\theta }\left(\begin{array}{c}\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots \end{array}\right)a{a}^T-\frac{1}{\theta }\left(\begin{array}{c}\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots \end{array}\right)I\\ & =\frac{1-\cos \theta }{\theta }{a}^{\wedge }+\frac{1}{\theta }\left(\begin{array}{c}\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots \end{array}\right)a{a}^T+\frac{1}{\theta }\left(\begin{array}{c}\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5+\cdots \end{array}\right)I\\ & =\frac{1-\cos \theta }{\theta }{a}^{\wedge }+\frac{1}{\theta }(\theta -\sin \theta )a{a}^T+\frac{\sin \theta }{\theta }I\\ & \triangleq J\end{array} \end{gathered}$$
式（3）得证。

5.伴随

证明$R{a}^{\wedge }{R}^T=(Ra{)}^{\wedge }$：
$$\begin{array}{rl}设，\exists c& =a^{\wedge }b\\ 则，\forall R\in SO(3)有Rc& =(Ra{)}^{\wedge }(Rb)\\ \Rightarrow c& =R^{-1}(Ra{)}^{\wedge }Rb\\ 因为c& =a^{\wedge }b\\ 所以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{array}$$
证明$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.

在上边链接中发现了居然还有这种性质，太好用了！发现自己就是小菜鸡！

$$\begin{gathered} 对R\exp (p^{\wedge })R^T求对数 \\ \begin{array}{rl}\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{array} \end{gathered}$$
注：这一题的证明我走了一些弯路，在阅读了链接中的提示和文献后还是一头雾水，始终没想到怎么用上图中的公式。网上看到了有人是下图这样证明的，但我感觉这样的证明法在第三行直接引入$R\cdot \exp (p^{\wedge })=\exp (Ad{j}_{R\cdot p^{\wedge }})\cdot R$，这一步我不太理解。希望助教老师如果时间充足的话可以帮忙看下，下图所示的证明方法是否正确？

img source:https://blog.csdn.net/jiahao62/article/details/80655542

6.常见函数的求导应用

1. 旋转点对旋转的导数。请分别通过左、右扰动的方式，计算$\frac{\partial Rp}{\partial R}$.

   存在两种解决方法：

   • 对R对应的李代数加上小量，求相对于小量的变化率（导数模型）；
   • 对R左乘或右乘一个小量，求相对于小量的李代数的变化率（扰动模型）。

   注：这里并不能按照矩阵微分来定义导数，$\frac{\partial Rp}{\partial R}$只是非正式的定义方式。

   左右扰动的原理: 对$R$进行一次扰动$\Delta R$，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点微小的差异。

   左扰动计算：

   设左扰动$\Delta R$对应的李代数为$\phi$，我们可以计算：
   $$\frac{\partial (Rp)}{\partial \phi }.$$
   则：
   $$\begin{array}{rl}\frac{\partial Rp}{\partial R}& =\frac{\partial (Rp)}{\partial \phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ 泰勒展开& \approx \underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }\\ 根据表1中提供的性质可得& =\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }\\ & =-(Rp{)}^{\wedge }\end{array}$$
   右扰动计算：

   同理，设右扰动$\Delta R$对应的李代数为$\phi$，我们可以计算：
   $$\frac{\partial (Rp)}{\partial \phi }.$$
   则：
   $$\begin{array}{rl}\frac{\partial Rp}{\partial R}& =\frac{\partial (Rp)}{\partial \phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })\exp ({\phi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ 泰勒展开& \approx \underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })(I+{\phi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge }){\phi }^{\wedge }p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{R{\phi }^{\wedge }p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{R{\phi }^{\wedge }R{R}^{T}p}{\phi }\\ 根据表1中提供的性质(Cu{)}^{\wedge }=C{u}^{\wedge }{C}^T可得:& \\ & =\underset{\phi \to 0}{\lim }\frac{(R\phi {)}^{\wedge }(R^{T}p)}{\phi }\\ 根据表1中提供的性质u^{\wedge }v=-v^{\wedge }u可得：& \\ & =\underset{\phi \to 0}{\lim }\frac{-(R^{T}p{)}^{\wedge }R\phi }{\phi }\\ & =-(R^{T}p{)}^{\wedge }R\end{array}$$

2. 旋转的复合。请分别通过左、右扰动的方式，计算：
   $$\begin{gathered} \frac{\partial \ln (R_1{R}_2{)}^{\vee }}{\partial R_1}. \\ 和 \\ \frac{\partial \ln (R_1{R}_2{)}^{\vee }}{\partial R_2}. \end{gathered}$$

   会使用链式法则，但是这里不太懂为什么需要将($R_1{R}_2$)转换成向量形式，也不知道如何处理下尖尖符号。所以下边的求导过程可能错漏百出，还望见谅。经过群里求助大佬，我又重新去看了下BHC公式，BHC公式告诉我们，当处理两个矩阵指数之积时，会产生一些由李括号组成的余项。

   左扰动：

   BHC线性近似表达：
   $$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{\mathcal{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& 当{\varphi }_1为小量时\\ {\mathcal{J}}_l({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& 当{\varphi }_2为小量时\end{array}$$
   左乘BHC近似雅克比${\mathcal{J}}_l$:
   $${\mathcal{J}}_l=\mathcal{J}=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{(1-\cos \theta )}{\theta }{a}^{\wedge }.$$
   它的逆：
   $${\mathcal{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}I+(1-\frac{\theta }{2}\cot \frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge }.$$
   通过左扰动的方式计算$\frac{\partial \ln(R_1R_2)^{\vee}}{\partial R_1}. $
   $$\begin{array}{rl}\frac{\partial \ln (R_1{R}_2{)}^{\vee }}{\partial R_1}& =\underset{{\phi }_1\to 0}{\lim }\frac{\ln (\exp ({\phi }_1^{\wedge })R_1{R}_2{)}^{\vee }-\ln (R_1{R}_2{)}^{\vee }}{{\phi }_1}\\ BHC近似& \approx \frac{{\mathcal{J}}_l(\ln (R_1{R}_2{)}^{\vee }{)}^{-1}{\phi }_1+\ln (R_1{R}_2{)}^{\vee }-\ln (R_1{R}_2{)}^{\vee }}{{\phi }_1}\\ & ={\mathcal{J}}_l(\ln (R_1{R}_2{)}^{\vee }{)}^{-1}\end{array}$$
   好像没有用到链式法则，尽力了，但是写不出来。

   通过右扰动的方式计算$\frac{\partial \ln (R_1{R}_2{)}^{\vee }}{\partial R_2}.$

   不会写

7.轨迹的描绘

1. 事实上，$T_{WC}$的平移部分即构成了机器人的轨迹。它的物理意义是什么？为何画出$T_{WC}$的平移
   部分就得到了机器人的轨迹？

   ​ 首先需要再回顾一下刚体运动的定义，两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动（课本p43）。而在刚体运动过程中，同一个向量在各个坐标系下的长度和夹角是不发生变化的。

   ​ 从物理的角度来理解向量这一概念，向量表示一种运动过程。假设摄像机就处于世界坐标系的原点位置（这样好理解一些，其实出于任意位置都可），用$T_{WC}$表示从相机坐标系到世界坐标系的转换，转换矩阵中包括旋转部分和平移部分。其中旋转部分可以将两个坐标系的x、y、z对齐，平移部分就表示相机的运动轨迹。所以画出$T_{WC}$的平移部分就得到了机器人的运动轨迹。思考了很久，最终只能理解到这个程度，也不知道对不对。

2. 完成数据读取部分代码，让程序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的错误提示。

​ 代码运行结果：

8.轨迹的误差

本题的代码参考了课本中的示例程序，详细代码和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