李群与李代数

习题一:群的性质

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

  1. $ \lbrace Z,+ \rbrace$ 是否为群?若是。验证其满足群定义;若不是,说明理由。
  2. $ \lbrace N, + \rbrace$ 是否为群?若是,验证其满足群定义;若不是,说明理由。 其中$Z$为整数集,$N$为自然数集。

答案

1、$ \lbrace Z,+ \rbrace$ 是群,群的性质验证:
封闭性:$ \forall Z_1,Z_2\in A, Z_1+Z_2 \in A $
结合律:$ \forall Z_1, Z_2, Z_3 \in A, (Z_1+Z_2) +Z_3 = Z_1+(Z_2+Z_3)$
幺元:$ \exists Z_0 = 0 \in A, Z_0+Z_1 = Z_1+Z_0 = Z_1 $
逆:$ \forall Z \in A \exists Z^{-1} = -Z \in A, s.t. Z+(-Z) = 0= Z_0$
2、 $ \lbrace N, + \rbrace$ 不是群,满足封闭性,结合律,幺元,但没有逆,原因如下:
封闭性:$ \forall N_1, N_2\in A, N_1 +N_2 \in A $
结合律:$ \forall N_1, N_2, N_3 \in A, (N_1+N_2) + N_3 = N_1 +(N_2 +N_3)$
幺元:$ \exists N_0 = 0 \ in A , N_0+N_1 = N_1+N_0 = N_1$
逆:$\forall N \in A, \exists N^{-1} = -N \notin A$

习题二:验证向量叉乘的李代数性质

我们说向量和叉乘运算构成了李代数,现在请你验证它。书中对李代数的定义为:李代数由一个集合$V$,一个数域$F$和一个二元运算$[, ]$组成。如果它们满足以下几条性质,称$(V,F,[,])$为一个李代数,记作$g$。

  1. 封闭性: $∀X, Y ∈ V, [X, Y ] ∈ V$
  2. 双线性:$∀X, Y , Z ∈ V, a, b ∈ F$,有: $$ [aX + bY , Z] = a[X, Z] + b[Y , Z], \ [Z, aX + bY ] = a[Z, X] + b[Z, Y ] $$
  3. 自反性:$ ∀X ∈ V, [X, X] = 0 $
  4. 雅可比等价:$ ∀X, Y , Z ∈ V, [X, [Y , Z]] + [Y , [Z, X]] + [Z, [X, Y ]] = 0 $

其中二元运算被称为李括号。 现取集合$V = R^3$,数域$F = R$,李括号为: $$ [a, b] = a × b. $$ 请验证$g = (R^3 , R, ×)$构成李代数。

答案

三维向量$R^3$上定义的叉积$×$是一种李括号,$g = (R^3 , R, ×)$构成了一个李代数,性质验证如下:
封闭性:$ [X,Y] = X \times Y \in R^3$
双线性:$ [aX+bY,Z] = (aX+bY) \times Z = a(X \times Z) + b(Y \times Z) $ $ [Z, aX+bY] = Z \times (aX + bY) = a(Z\times X) + b(Z\times Y) $
自反性:$ [X,X] = X \times X = 0 $
雅克比等价性:$ [X, [Y,Z]] + [Y, [Z,X]] + [Z, [X,Y]] = X\times Y \times Z + Y\times Z\times X + Z\times X \times Y = 0 $ 故向量的叉乘满足李代数的性质。

习题五:轨迹的描绘

我们通常会记录机器人的运动轨迹,来观察它的运动是否符合预期。大部分数据集都会提供标准轨迹以供参考,如kittiTUM-RGBD等。这些文件会有各自的格式,但首先你要理解它的内容。记世界坐标系为$W$,机器人坐标系为$C$,那么机器人的运动可以用$T_ {WC}$或$T_ {CW}$来描述。现在,我们希望画出机器人在世界当中的运动轨迹,请回答以下问题:

  1. 事实上,$T_ {WC}$的平移部分即构成了机器人的轨迹。它的物理意义是什么?为何画出$T_ {WC}$的平移部分就得到了机器人的轨迹?
  2. 我为你准备了一个轨迹文件(code/trajectory.txt)。该文件的每一行由若干个数据组成,格式为

$$ [t, t_x , t_y , t_z , q_x , q_y , q_z , q_w ] $$

其中$t$为时间,$t_ x , t_ y , t_ z$ 为$T_ {WC}$的平移部分,$q_ x , q_ y , q_ z , q_ w$是四元数表示的$T_ {WC}$的旋转部分,$q_ w$为四元数实部。同时,我为你提供了画图程序draw_trajectory.cpp文件。该文件提供了画图部分的代码,请你完成数据读取部分的代码,然后书写CMakeLists.txt以让此程序运行起来。注意我们需要用到Pangolin库来画图,所以你需要事先安装Pangolin(如果你做了第一次作业,那么现在已经安装了)。CMakeLists.txt可以参照ORB-SLAM2部分。

提示

  1. $T_ {wc}$表示相机坐标系到世界坐标系的转换,一般$R$和$T$都是从右向左的顺序表示。
  2. $T_ {wc}$中的平移部分可以直接看到相机在何处,这也是说$T_ {wc}$更为直观的原因。
  3. SLAM中的变换矩阵的实际操作顺序是先旋转再平移。

答案

1、$T_ {wc}$表示相机坐标系到世界坐标系的转换,其中平移部分的物理意义表示经过旋转矫正后的相机坐标系的原点在世界坐标系中的坐标位置,进而相机坐标系的原点就是代表了机器人,所以 $T_ {wc}$的平移部分就是机器人在世界坐标系中的位置轨迹。
2、变换矩阵的下标一般是从右向左表示,这一点需要注意。另外,如果表示的是从世界到相机坐标系的转换矩阵中的平移不在是坐标,这时候变换矩阵的逆,形式发生了变化。

最终轨迹如图所示:

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

相关代码如下:

    int main(int argc, char **argv) 
    {
        vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

        ifstream trajectory;
        trajectory.open(trajectory_file.c_str());

        if(!trajectory)
        {
            cout<<"open file error!"<<endl;
            return -1;
        }
        string trajectoryLine;
        while(getline(trajectory, trajectoryLine) && !trajectoryLine.empty())
        {
            istringstream templine(trajectoryLine);
            templine>>timestamp>>t[0]>>t[1]>>t[2]>>q.x()>>q.y()>>q.z()>>q.w();
            T = Sophus::SE3(q,t);
            poses.push_back(T);
        }
        trajectory.close();
        DrawTrajectory(poses);
        return 0;
    }

习题六:轨迹的误差

除了画出真实轨迹以外,我们经常需要把SLAM估计的轨迹与真实轨迹相比较。下面说明比较的原理,请你完成比较部分的代码实现。
设真实轨迹(ground-truth)为$T_ g$,估计轨迹$T_ e$。它们都以$T_ {WC}$的形式存储,格式同上题。现在,你需要计算估计轨迹的误差。我们假设每一个$T_ g$都与给定的$T_ e$对应。那么,对于任意第$i$个位姿,它的误差可定义为:

$$ e_{i}=\left\|\log \left(\boldsymbol{T}_{g i}^{-1} \boldsymbol{T}_{e i}\right)^{\mathrm{v}}\right\|_{2} $$

即两个位姿之差的李代数二范数。于是,可以定义两条轨迹的均方根(Root-Mean-Square-Error,RMSE)误差为:

$$ \operatorname{RMSE}(g, e)=\sqrt{\frac{1}{n} \sum_{i=1}^{n} e_{i}^{2}} $$

我为你准备了code/ground-truth.txtcode/estimate.txt两条轨迹。请你根据上面公式,实现RMSE的计算代码,给出最后的RMSE结果。作为验算,参考答案为:2.207。

提示

  1. 实际当中的轨迹比较还要更复杂一些。通常ground-truth由其他传感器记录(如vicon),它的采样频率通常高于相机的频率,所以在处理之前还需要按照时间戳对齐。另外,由于传感器坐标系不一致,还需要计算两个坐标系之间的差异。这件事也可以用ICP解得,我们将在后面的课程中讲到。
  2. 你可以用上题的画图程序将两条轨迹画在同一个图里,看看它们相差多少。

答案

1、在视觉里程计1中,会有坐标系对比的习题,是3D到3D的转换。
2、李代数中的顺序是先平移,再旋转。这一点需要注意,G2O中不同,所以我们在适用G2O设置雅各比矩阵的时候,十四讲中的扰动模型的导数,需要将前三列和后三列对换。但是,在接下来的习题中,我们大多使用EigenSophus自己手写优化问题,所以雅各比矩阵和十四讲中的一致。

最终结果为(和正确答案一样):

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

相关代码如下:

int main(int argc, char **argv) 
{
    // vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

    ifstream trajectory_g;
    ifstream trajectory_e;
    trajectory_g.open(trajectory_file_g.c_str());
    trajectory_e.open(trajectory_file_e.c_str());

    if(!trajectory_g || ! trajectory_e)
    {
        cout<<"open file error!"<<endl;
        return -1;
    }
    int num = 0;
    string trajectoryLine_g;
    string trajectoryLine_e;
    while(getline(trajectory_g, trajectoryLine_g) && !trajectoryLine_g.empty() &&
            getline(trajectory_e,trajectoryLine_e) && !trajectoryLine_e.empty())
    {
        istringstream templine_g(trajectoryLine_g);
        istringstream templine_e(trajectoryLine_e);
        templine_g>>timestamp_g>>tg[0]>>tg[1]>>tg[2]>>qg.x()>>qg.y()>>qg.z()>>qg.w();
        templine_e>>timestamp_e>>te[0]>>te[1]>>te[2]>>qe.x()>>qe.y()>>qe.z()>>qe.w();
        Tg = Sophus::SE3(qg,tg);
        Te = Sophus::SE3(qe,te);

        kesi = (Tg.inverse()*Te).log();
        error += kesi.transpose()*kesi;
        num++; 
    }
    trajectory_g.close();
    trajectory_e.close();
    RMSE = sqrt(error/num);
    cout<<"RMSE: "<<RMSE<<endl;
    return 0;
}