视觉SLAM十四讲：李群和李代数Sophus库相关课后程序理解

1. Sophus库基础应用

//
// Created by g214-j on 18-7-22.
//

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"


int main(int argc, char** argv){

    // 沿Z轴转90度的旋转矩阵(由旋转轴得到旋转矩阵)
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    // SO3特殊正交群变量的创建
    Sophus::SO3 SO3_R(R);               // 由旋转矩阵直接构建SO3特殊正交群
    Sophus::SO3 SO3_v(0,0,M_PI/2);      // 也可以直接由旋转向量构建SO3特殊正交群
    Eigen::Quaterniond q(R);            // 四元数
    Sophus::SO3 SO3_q(q);               // 也可以直接由四元数构建SO3特殊正交群

    // SO3李群的输出，输出将会以构建李群时的原始方式so3表示，要注意：这里不是矩阵
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from vector: "<<SO3_v<<endl;
    cout<<"SO(3) from quaternion: "<<SO3_q<<endl;

    // 使用对数映射获得相应的李代数(特殊正交群对应的李代数其实是旋转向量，即三维向量)
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // 向量的反对称形式
    cout<<"so3 hat = \n"<<Sophus::SO3::hat(so3)<<endl;
    // 反对称回到向量
    cout<<"so3 hat vee = \n"<<Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose()<<endl;

    // 使用扰动模型进行微小的变动
    Eigen::Vector3d update_so3(1e-4,0,0);       // 假设更新量为这么多
    Sophus::SO3 SO3_update = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 update = "<<SO3_update<<endl;        // 输出还是so3的形式
    cout<<"SO3 update（in matrix model） = "<<endl<<SO3_update.matrix()<<endl;

    /********************萌萌的分割线*****************************/
    cout<<"************我是分割线*************"<<endl;

    // SO3操作完了，对SE3进行类比操作
    Eigen::Vector3d t(1,0,0);       // 平移量，沿x轴平移1
    Sophus::SE3 SE3_Rt(R,t);        // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);        // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;     // 好奇怪，这里的输出形式和se3的输出形式不一样，这里旋转在前，平移在后
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;

    // SE3对应的李代数为6维的向量
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    // 观察输出后，会发现在Sophus中，se(3)的平移在前，旋转在后！！！

    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;          // se3对应的hat为一个四维的矩阵，这里的hat和vee和李代数对应的群有关，所以就能理解hat和vee函数要放在对应的李群类中调用了
    cout<<"se3 hat vee = "<<endl<<Sophus::SE3::vee(Sophus::SE3::hat(se3))<<endl;

    // 最后，演示一下扰动更新
    Vector6d update_se3;    // 创建李代数上的微小更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4; // 确定微小更新量的值
    Sophus::SE3 SE3_update = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 update = "<<endl<<SE3_update.matrix()<<endl;         // 这里输出要调用变量自身的matrix（）函数转换成矩阵形式打印输出，猜测可能是变量类内部实现的时候的存储方式不是矩阵，所以要转换一下

    return 0;
}

 

2. 轨迹的描绘

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>

// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file = "./trajectory.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);

int main(int argc, char **argv) {

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

    /// implement pose reading code
    // start your code here (5~10 lines)

    fstream fin("./trajectory.txt");
    string line;
    double t, tx, ty, tz, qx, qy, qz, qw;
    while( getline(fin,line) ){
        stringstream lineStream(line);
        lineStream>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Eigen::Vector3d t(tx,ty,tz);
        Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();
        Sophus::SE3 SE3_qt(q,t);
        poses.push_back(SE3_qt);
    }

    // end your code here

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_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)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));


    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
(qw,qx,qy,qz)
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[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);   // sleep 5 ms
    }

}

 

3. 计算轨迹的误差

//
// Created by g214-j on 18-7-22.
//

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pangolin/pangolin.h>

using namespace std;


void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1);
void ReadData(string fileName, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>& pose);
double error_Twc(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Tg, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Te);

// 路径变量
string estimated_file = "./estimated.txt";
string groundtruth_file = "./groundtruth.txt";
string trajectory_file = "./trajectory.txt";


int main(int argc, char** argv){

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_estimated;
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_groundtruth;
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_trajectory;

    ReadData(estimated_file, pose_estimated);
    ReadData(groundtruth_file, pose_groundtruth);
    ReadData(trajectory_file, pose_trajectory);

    cout<<"The size of estimated data is "<<pose_estimated.size()<<endl;
    cout<<"The size of groundtruth data is "<<pose_groundtruth.size()<<endl;
    cout<<"The size of trajectory data is "<<pose_trajectory.size()<<endl;

    double trajectory_error_RMSE = 0;
    trajectory_error_RMSE = error_Twc(pose_groundtruth, pose_estimated);
    cout<<"trajectory_error_RMSE = "<<trajectory_error_RMSE<<endl;

    DrawTrajectory(pose_estimated,pose_groundtruth);


    return 0;
}




/*******************************************************************************************/
// 绘制两条曲线
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1)
{
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_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)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));


    while (pangolin::ShouldQuit() == false) {
        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);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);

            // 第二条线绘制
            glColor3f(0.f, 0.8f, 0.f);
            glBegin(GL_LINES);
            auto p3 = poses1[i], p4 = poses1[i + 1];
            glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
            glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);

            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}


/*******************************************************************************************/
// 读取文件数据，转换成SE3数据类型，存进pose向量中
void ReadData(string fileName, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>& pose){

    ifstream trajectory(fileName);
    if(!trajectory.is_open()){
        cout<<"No "<<fileName<<endl;
        return;
    }

    double t, tx, ty, tz, qx, qy, qz, qw;
    string line;
    while(getline(trajectory,line)){
        stringstream lineStream(line);
        lineStream>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Eigen::Vector3d t(tx,ty,tz);
        Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();
        Sophus::SE3 SE3_qt(q,t);
        pose.push_back(SE3_qt);
    }

    return;
}



/*******************************************************************************************/
// 计算两条曲线的RMSE误差，输入为两条曲线的SE3向量，输出为RMSE误差值
double error_Twc(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Tg, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Te){

    vector<double> error;
    Eigen::Matrix<double,6,1> se3;
    // 计算路径上每一对对应点的误差
    for(int i=0;i<Tg.size();i++){

        se3 = Sophus::SE3::log(Tg[i].inverse() * Te[i]);
        cout<<"se3 = "<<se3.transpose()<<endl;
        error.push_back(se3.squaredNorm());         // 二范数,已经是每一对误差的平方
        cout<<"se3 squarNorm is "<<error[i]<<endl;
    }

    double RMSE = 0;
    for(int i =0;i<error.size();i++){

        RMSE+=error[i];
    }

    RMSE/=(double)error.size();
    RMSE = sqrt(RMSE);

    return RMSE;
}