三维空间中的刚体运动

习题一:Eigen矩阵运算

设线性方程组$Ax=b$,在$A$为方阵的前提下,回答以下问题:
1、在什么条件下,$x$有解且解唯一?
2、高斯消元法的原理是什么?
3、$QR$分解的原理是什么?
4、$Cholesky$分解的原理是什么?
5、编程实现A为$100*100$随机矩阵,用$QR$分解和$Cholesky$分解求$x$的程序。

答案:

1、系数矩阵$A$的秩为$n$时,即$det(A)$不为$0$的情况下,方程有唯一解。
2、高斯消元法的原理是线性方程组的初等变换。
3、$QR$分解是三种将矩阵分解的方式之一,$QR$分解是将矩阵分解为一个正交矩阵$Q$与一个上三角矩阵$R$的积,其中正交矩阵:$Q^T = Q^{-1}$。$QR$分解经常用来求解线性最小二乘问题。$QR$分解的实际计算方法有很多种,例如Givens旋转、Householder变换以及Gram-Schmidt正交化等等。使用$QR$分解进行线性方程组的求解:$Ax=b \implies QRx=b \implies Rx=Q^Tb$其中$R$为上三角矩阵,因此很容易求得方程组的解。
4、Cholesky(楚列斯基分解)是指将一个正定的Hermite矩阵分解成一个下三角矩阵与其共轭转置矩阵的乘积。公式表述为

$$ A = LL^* $$

其中$L$是一个下三角矩阵且所有对角元素均为正实数。当A为实数矩阵的时候,此时`Cholesky`分解可以改写为$A=LL^T$;当A为正定矩阵的时候,`Cholesky`分解是唯一的,即存在一个对角元素均严格大于零的下三角矩阵,使得$A=LL^*$成立,但是$A$半正定的时候,分解则不一定唯一。 正定:一个是实对称矩阵$M$是正定的,当且仅当对于所有的非零实系数向量$Z$,都有$Z^TMZ>0$。$M$的所有特征值都是正的。 `Hermite`矩阵:埃尔米特矩阵,自伴随矩阵,是共轭对称的方阵。矩阵中的第$i$行第$j$列元素都是第$j$行第$i$列的元素复共轭。$A=A^H$,如下所示,`Hermite`矩阵的对角线上的元素都是实数,其特征值也是实数。实对称矩阵是`Hermiter`矩阵的特例。

$$ \begin{bmatrix} 3 & 2+i \\ 2-i & 1 \\ \end{bmatrix} $$

5、Cholesky分解对于正定对称矩阵有效,所以要先将矩阵变换为正定对称矩阵再进行比较。在程序中有进行变换和未进行变化的比较;Eigen固定大小的矩阵最大到50,所以我们会使用动态大小的矩阵,程序中也有固定大小和动态矩阵时间上的差异比较。

程序最终的运行结果为:

完整程序链接:https://github.com/XLMaverick/Visual-Localization-Percessing/tree/master/vslam_fourteen_lectures/EigenDense

代码如下:

    #include<iostream>
    #include<ctime>
    #include<Eigen/Core>
    #include<Eigen/Dense>

    using namespace std;
    #define SIZE 100

    int main(int argc, char** argv)
    {
        /* Eigen最大的固定数组为50,当超过50时,如果没有使用动态初始化的方式,可以运行,但是时间效率非常的慢*/
        // Eigen::Matrix<double, SIZE, SIZE> matrixA;
        // Eigen::Matrix<double, SIZE,1> matrixb;

        //两种动态动态初始化的方式。
        // Eigen::MatrixXd matrixA;
        // Eigen::MatrixXd matrixb;

        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrixA;
        Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrixb;

        matrixA = Eigen::MatrixXd::Random(SIZE,SIZE);
        matrixb = Eigen::MatrixXd::Random(SIZE,1);

        /*将系数矩阵变换为正定对称矩阵,如果没有这一步,Cholesky分解与前两个结果不一致 */
        matrixA = matrixA.transpose()*matrixA;

        Eigen::Matrix<double,SIZE,1> x;
        clock_t time_sst = clock();
        x = matrixA.inverse()*matrixb;
        cout<<x.transpose()<<endl;
        cout<<"time use in normal inverse is "<<1000*(clock()-time_sst)/(double)CLOCKS_PER_SEC<<"ms"<<endl;


        time_sst = clock();
        x= matrixA.colPivHouseholderQr().solve(matrixb);
        cout<<x.transpose()<<endl;
        cout<<"time use in QR decomposition is "<< 1000*(clock()-time_sst)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

        time_sst = clock();
        x = matrixA.ldlt().solve(matrixb);
        cout<<x.transpose()<<endl;
        cout<<"time use in Cholesky decomposition is "<<1000*(clock()-time_sst)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

        return 0;
    }

习题二:几何运算练习

设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为: $q_ 1 = [0.55,0.3,0.2,0.2]$,$t_ 1 =[0.7,1.1,0.2]^T$ ($q$的第一项为实部)。这里的$q$和$t$表达的是$T_ {cw}$,也就是世界到相机的变换关系。小萝卜二号的位姿为$q_ 2 = [−0.1,0.3,−0.7,0.2]$,$t_ 2 = [−0.1,0.4,0.8]^T$。现在,小萝卜一号看到某个点在自身的坐标系下,坐标为$p_1 = [0.5,−0.1,0.2]^T$,求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交你的程序。
提示:

  1. 四元数在使用前需要归一化。
  2. 请注意Eigen在使用四元数时的实部和虚部的顺序。

答案:

1、因为单位四元数才能表示旋转,所以使用四元数前必须进行归一化。
2、四元数定义时和打印时的顺序不一致。
3、变换矩阵定义旋转矩阵和平移矩阵的函数需要注意,rotate() pretranslate()

最终结果为:

完整程序链接:https://github.com/XLMaverick/Visual-Localization-Percessing/tree/master/vslam_fourteen_lectures/EigenGeometry

代码如下:

    #include<iostream>
    #include<Eigen/Core>
    #include<Eigen/Geometry>
    using namespace std;
    int main(int argc, char** argv)
    {
        /*四元数定义的顺序和打印的顺序不一致,需要注意*/
        Eigen::Quaterniond q1(0.7,0.3,0.2,0.2);
        cout<<"quaterniond q1 "<<q1.coeffs();
        Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
        /*单位四元数才能描述旋转,所以使用四元数前必须进行归一化*/
        T1.rotate(q1.normalized().toRotationMatrix());
        T1.pretranslate(Eigen::Vector3d(0.7,1.1,0.2));

        Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2);
        cout<<"quaterniond q2 "<<q2.coeffs();
        Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
        T2.rotate(q2.normalized().toRotationMatrix());
        T2.pretranslate(Eigen::Vector3d(-0.1,0.4,0.8));

        Eigen::Vector3d p1(0.5,-0.1,0.2);
        Eigen::Vector3d p2 = Eigen::Vector3d::Identity();

        p2 = T2*T1.inverse()*p1;

        cout<<"position in camera2 is "<<p2.transpose()<<endl;
        return 0;
    }

习题三:旋转的表达

课程中提到了旋转可以用旋转矩阵、旋转向量与四元数表达,其中旋转矩阵与四元数是日常应用中常见的表达方式。请根据课件知识,完成下述内容的证明。

  1. 设有旋转矩阵$R$,证明$R^TR = I$且$detR= 1$。
  2. 设有四元数$q$,我们把虚部记为$ε$,实部记为$η$,那么$q=(ε,η)$。请说明$ε$和$η$的维度。
  3. 定义运算$+$和$\bigoplus$为:

$$ \boldsymbol{q}^{+}=\left[\begin{array}{cc}{\eta \mathbf{1}-\boldsymbol{\varepsilon}^{ \times}} & {\boldsymbol{\varepsilon}} \\ {-\boldsymbol{\varepsilon}^{\mathrm{T}}} & {\eta}\end{array}\right] , \boldsymbol{q}^{\oplus}=\left[\begin{array}{cc}{\eta \mathbf{1}+\varepsilon^{ \times}} & {\varepsilon} \\ {-\varepsilon^{\mathrm{T}}} & {\eta}\end{array}\right] $$

请证明对任意单位四元数$q_1,q_2$,四元数乘法可写成矩阵乘法:

$$ q_1·q_2 = q_1^+q_2 $$ 或者

$$ q_1·q_2 = q_2^⊕q_1 $$

答案:

1、设两个坐标系的基坐标分别为$\begin{bmatrix} e_1,&e_2,& e_3\end{bmatrix}^T$,$\begin{bmatrix} e_1^{\prime},& e_2^{\prime},& e_3^{\prime} \end{bmatrix}^T$,则我们可以推导出旋转矩阵:

$$ R = \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1^{\prime},& e_2^{\prime},& e_3^{\prime} \end{bmatrix} $$

则$R^TR$为:

$$ R^TR = \begin{bmatrix} e_1^{'T} \\ e_2^{'T} \\ e_3^{'T} \end{bmatrix} \begin{bmatrix} e_1,&e_2,& e_3\end{bmatrix} \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1^{'},& e_2^{'},& e_3^{'} \end{bmatrix} $$

分析$R^TR$的中间两项:

$$ \begin{bmatrix} e_1,&e_2,& e_3\end{bmatrix} \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} $$

由于是基坐标,两两相互正交且为单位向量,所以我们容易验证中间两项的成绩为单位阵$I$。 则$R^TR$可以化简为:

$$ R^TR = \begin{bmatrix} e_1^{'T} \\ e_2^{'T} \\ e_3^{'T} \end{bmatrix} \begin{bmatrix} e_1,&e_2,& e_3\end{bmatrix} \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1^{'},& e_2^{'},& e_3^{'} \end{bmatrix}= \begin{bmatrix} e_1^{'T} \\ e_2^{'T} \\ e_3^{'T} \end{bmatrix} \begin{bmatrix} e_1^{'},& e_2^{'},& e_3^{'} \end{bmatrix} = I $$

最后行列的证明:

$$ |R^TR| = |R^T||R| = |R||R| = 1 $$

则我们可以推出$detR=+1$。 2、四元数的实部的维度为1,虚部的维度为3。 3、根据上述我们可以得知

$$ q_1= \begin{bmatrix} ε_1 \\ η_1 \end{bmatrix} q_2= \begin{bmatrix} ε_2 \\ η_2 \end{bmatrix} $$

根据四元数的乘法我们可以得到:

$$ q_1q_2 = \begin{bmatrix} ε_1η_2+ε_2η_1+ε_1 \times ε_2 \\ η_1η_2- ε_1^Tε_2 \end{bmatrix}=\begin{bmatrix} η_1I+ε_1^{\times} & ε_1 \\ -ε_1^T & η_1\end{bmatrix} \begin{bmatrix} ε_1 \\ η_1\end{bmatrix}= q_2^⊕q_1 $$

同理也可以得到:

$$ q_1q_2 = \begin{bmatrix} ε_1η_2+ε_2η_1+ε_1\timesε_2\\η_1η_2- ε_1^Tε_2\end{bmatrix}=\begin{bmatrix} η_2I-ε_2^{\times} & ε_2\\-ε_2^T & η_2\end{bmatrix} \begin{bmatrix} ε_1\\ η_1\end{bmatrix}= q_1^+q_2 $$

习题四:四元数运算性质的验证

课程中介绍了单位四元数可以表达旋转。其中,在谈论用四元数$q$旋转点$p$时,结果为:

$$ p^{′} = qpq^{−1} $$ 我们说,此时$p^′$ 必定为虚四元数(实部为零)。请你验证上述说法。 此外,上式亦可写成矩阵运算:$p^′=Qp$。请根据你的推导,给出矩阵$Q$。注意此时$p$和$p^′$都是四元数形式的变量,所以$Q$为4 × 4的矩阵。 提示:如果使用第4题结果,那么有:

$$ p^′ = qpq^{−1} = q^+ p^+ q^{−1} = q^+q^{−1^{⊕}}v $$ 从而可以导出四元数至旋转矩阵的转换方式:

$$ R = q^+ q^{−1^⊕} . $$

答案:

先挖坑,以后有空再填了,markdown编写公式太费时间了啊!!!!

习题五:罗德里格斯公式的证明

罗德里格斯公式描述了从旋转向量到旋转矩阵的转换关系。设旋转向量长度为$θ$,方向为$n$,那么旋转矩阵$R$为: $$ R = cosθI − (1−cosθ)nn^T + sinθn^∧ . $$ 我们在课程中仅指出了该式成立,但没有给出证明。请你证明此式。 提示:参考 https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula。

答案:

挖坑挖坑,这一点李群$SO(3)$的指数映射也会用到啊,可能后面VIO专题会补上吧。

$$ \begin{array}{c}{\exp \phi^{\wedge}=\exp (\theta \mathbf{a})=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\theta \mathbf{a}^{\wedge}\right)^{n}} \ {=\cos \theta \mathbf{I}+(1-\cos \theta) \mathbf{a} \mathbf{a}^{T}+\sin \theta \mathbf{a}^{T}}\end{array} $$