【SLAM】视觉SLAM十四讲（三：李群与李代数）

参考https://blog.csdn.net/weixin_41074793/article/details/84853291

轨迹描绘

1.事实上,T W C 的平移部分即构成了机器人的轨迹。它的物理意义是什么?为何画出 T W C 的平移
部分就得到了机器人的轨迹?
坐标的变换就是Twc的平移部分，我理解的物理意义就是机器人从某一坐标移到了另一坐标，因此平移部分就是机器人的轨迹。
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 W C 的平移部分,q x , q y , q z , q w 是四元数表示的 T W C 的旋转部分,q w
为四元数实部。同时,我为你提供了画图程序 draw_trajectory.cpp 文件。该文件提供了画图部分
的代码,请你完成数据读取部分的代码,然后书写 CMakeLists.txt 以让此程序运行起来。注意我
们需要用到 Pangolin 库来画图,所以你需要事先安装 Pangolin(如果你做了第一次作业,那么现
在已经安装了)。CMakeLists.txt 可以参照 ORB-SLAM2 部分。

draw_trajectory.cpp

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>
#include <unistd.h> 
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

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

using namespace std;

// path to trajectory file
string trajectory_file = "/home/huangyuan/slam/slambook/pa3/drawtrace/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)
    //读取txt文件
     // implement pose reading code
    ifstream infile(trajectory_file);
    double t1,tx,ty,tz,qx,qy,qz,qw;
    string line;
    if(infile)
    {
        while(getline(infile,line))
        {
            stringstream record(line);    //从string读取数据
            record>>t1>>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);
        }
    }
    else
   {
       cout<<"没找到这个文件"<<endl;
   }
         
    infile.close();
    // end your code here

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

/*******************************************************************************************/
void DrawTrajectory(vec