第4讲 李群与李代数

1 Sophus的基本使用

  cpp文件内容如下，

#include 
#include 
#include 

using namespace std;

#include       //Eigen核心模块
#include   //Eigen几何模块

using namespace Eigen;

#include "sophus/se3.hpp"

using namespace Sophus;

typedef Eigen::Matrix<double, 6, 1> Vector6d;

clock_t start, last;

//本程序演示sophus的基本用法
int main()
{
     
    start = clock();
    cout << "对SO3和so3的操作如下：" << endl;
    //沿Z轴旋转90度的旋转矩阵
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    //用旋转矩阵初始化的四元数
    Quaterniond q(R);

    //从旋转矩阵构造正交群SO3
    SO3d SO3_R(R);
    //从四元数构造正交群SO3
    SO3d SO3_q(q);
    cout << "SO3_R和SO3_q是相等的！" << endl;
    cout << "SO3_R = \n" << SO3_R.matrix() << endl;
    cout << "SO3_q = \n" << SO3_q.matrix() << endl;

    //使用对数映射获得正交群SO3的李代数so3
    Vector3d so3 = SO3_R.log();
    cout << "so3 = \n" << so3 << endl;
    //hat表示从三维向量到反对称矩阵
    cout << "so3的反对称矩阵为：\n" << SO3d::hat(so3) << endl;
    //vee表示从反对称矩阵到三维向量
    cout << "so3 = \n" << SO3d::vee(SO3d::hat(so3)) << endl;

    //增量扰动模型的更新
    Vector3d update_so3(1e-4, 0, 0);   //更新量
    SO3d SO3_updated = SO3d::exp(update_so3) * SO3_R;
    cout << "经扰动增量更新之后的正交群SO3为：\n" << SO3_updated.matrix() << endl;

    cout << "****************************************************************************************************************" << endl;

    cout << "对SE3和se3的操作如下：" << endl;
    Vector3d t(1, 0, 0);  //沿X轴平移1个单位
    SE3d SE3_Rt(R, t);    //用R和t初始化SE3
    SE3d SE3_qt(q, t);    //用q和t初始化SE3
    cout << "SE3_Rt和SE3_qt是相等的！" << endl;
    cout << "SE3_Rt = \n" << SE3_Rt.matrix() << endl;
    cout << "SE3_qt = \n" << SE3_qt.matrix() << endl;

    Vector6d se3 = SE3_Rt.log();
    cout << "se3 = \n" << se3 << endl;
    cout << "观察se3输出可知，在Sophus库中se(3)的平移在前，旋转在后！" << endl;
    //hat表示从四维向量到反对称矩阵
    cout << "se3的反对称矩阵为：\n" << SE3d::hat(se3) << endl;
    //vee表示从反对称矩阵到四维向量
    cout << "se3 = \n" << SE3d::vee(SE3d::hat(se3)) << endl;

    //增量扰动模型的更新
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0, 0) = 1e-4d;
    SE3d SE3_updated = SE3d::exp(update_se3) * SE3_Rt;
    cout << "经扰动增量更新之后的欧式群SE3为：\n" << SE3_updated.matrix() << endl;

    last = clock();
    cout << "总共花费的时间为：" << 1000 * double(last - start) / CLOCKS_PER_SEC << "毫秒！" << endl;

    return 0;
}

  CMakeLists.txt文件内容如下，

find_package(Sophus REQUIRED)
include_directories(${
     Sophus_INCLUDE_DIRECTORIES})

include_directories("/usr/include/eigen3")

add_executable(main main.cpp)

  结果如下，

对SO3和so3的操作如下：
SO3_R和SO3_q是相等的！
SO3_R = 
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
SO3_q = 
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
so3 = 
     0
     0
1.5708
so3的反对称矩阵为：
      0 -1.5708       0
 1.5708       0      -0
     -0       0       0
so3 = 
     0
     0
1.5708
经扰动增量更新之后的正交群SO3为：
          0          -1           0
          1           0     -0.0001
     0.0001 2.03288e-20           1
****************************************************************************************************************
对SE3和se3的操作如下：
SE3_Rt和SE3_qt是相等的！
SE3_Rt = 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
SE3_qt = 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
se3 = 
 0.785398
-0.785398
        0
        0
        0
   1.5708
观察se3输出可知，在Sophus库中se(3)的平移在前，旋转在后！
se3的反对称矩阵为：
        0   -1.5708         0  0.785398
   1.5708         0        -0 -0.785398
       -0         0         0         0
        0         0         0         0
se3 = 
 0.785398
-0.785398
        0
        0
        0
   1.5708
经扰动增量更新之后的欧式群SE3为：
2.22045e-16          -1           0      1.0001
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
总共花费的时间为：0.838毫秒！

2 评估轨迹的误差

  cpp文件内容如下，

#include 
#include 
#include 
#include 

using namespace std;

#include 

using namespace Sophus;

#include 

string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";

typedef vector<SE3d, Eigen::aligned_allocator<SE3d>> TrajectoryType;

TrajectoryType ReadTrajectory(const string& path);
void DrawTrajectory(const TrajectoryType& gt, const TrajectoryType& esti);

clock_t start, last;

int main()
{
     
    start = clock();

    TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
    TrajectoryType estimated = ReadTrajectory(estimated_file);

    //assert()为断言函数。如果它的条件返回错误，则终止程序执行！
    assert(!groundtruth.empty() && !estimated.empty());
    assert(groundtruth.size() == estimated.size());

    //计算绝对轨迹误差，即为李代数的均方根误差
    double rmse = 0;
    for(int i = 0; i < estimated.size(); i++)
    {
     
        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 << endl;

    DrawTrajectory(groundtruth, estimated);

    last = clock();
    cout << "花费的时间为" << (double)(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒！" << endl;
    return 0;
}


TrajectoryType ReadTrajectory(const string& path)
{
     
    ifstream fin(path);
    TrajectoryType trajectory;
    if(!fin)  //如果文件打开失败，则执行
    {
     
        //cerr是标准错误流。该流中的信息只能输出到显示器上，而不能输出到文件中。该流中信息不经过缓冲区，直接输出至显示器。
        cerr << "没有找到文件" << path << "！" << endl;
        return trajectory;
    }

    while(!fin.eof())  //当fin并未指向EOF时，则执行
    {
     
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        SE3d p1(Eigen::Quaterniond(qx, qy, qz, qw), Eigen::Vector3d(tx, ty, tz));
        trajectory.push_back(p1);
    }

    return trajectory;
}


void DrawTrajectory(const TrajectoryType& gt, const TrajectoryType& esti)
{
     
    //创建pangolin窗口
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);  //启用深度渲染，根据目标的远近自动隐藏被遮挡的模型
    glEnable(GL_BLEND);  //窗口使用颜色混合模式，让物体显示半透明效果
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    //GL_SRC_ALPHA表示使用源颜色的alpha值作为权重因子，GL_ONE_MINUS_SRC_ALPHA表示用1.0-源颜色的alpha值作为权重因子

    //创建一个观察相机视图
    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
            );
    //ProjectionMatrix()中的参数依次为相机视图宽度、相机视图高度、fx、fy、cx、cy、深度最小值和深度最大值
    //ModelViewLookAt()中的参数依次为相机的位置，被观察点位置，右上前为相机坐标系中XYZ轴的正方向

    //创建交互视图，用于显示上一步相机所观测到的内容
    pangolin::Handler3D handler(s_cam);  //交互相机视图句柄
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(&handler);
    //SetBounds()参数依次为交互视图的bottom、top、left、right和其横纵比

    while(pangolin::ShouldQuit() == false)  //如果pangolin窗口没有关闭，则执行
    {
     
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);  //清空颜色和深度缓存，使得前后帧不会互相干扰
        d_cam.Activate(s_cam);  //激活显示，并设置相机状态
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);  //设置背景颜色为白色
        glLineWidth(2);  //设置线条宽度为2

        //绘制真实轨迹，即循环画线段
        for(int i = 0; i < gt.size() - 1; i++)
        {
     
            glColor3f(0.0f, 0.0f, 1.0f);  //颜色设置为蓝色
            glBegin(GL_LINES);
            SE3d p1 = gt[i], p2 = gt[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        //绘制估计轨迹，即循环画线段
        for(int i = 0; i < esti.size(); i++)
        {
     
            glColor3f(1.0f, 0.0f, 0.0f);  //颜色设置为红色
            glBegin(GL_LINES);
            SE3d p1 = esti[i], p2 = esti[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();  //执行渲染，即按照前面的设置进行最终的显示
        usleep(5000);  //停止执行5毫秒
    }

}

  CMakeLists.txt文件内容如下，

find_package(Sophus REQUIRED)
include_directories(${
     Sophus_DIRECTORIES})

find_package(Pangolin REQUIRED)
include_directories(${
     Pangolin_DIRECTORIES})

add_executable(main main.cpp)

target_link_libraries(main ${
     Pangolin_LIBRARIES})

  结果如下，

绝对轨迹误差为：2.20728
花费的时间为2826.6毫秒！