高翔第三讲轨迹的描绘

高翔第三讲轨迹的描绘

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

1. 事实上,T W C 的平移部分即构成了机器人的轨迹。它的物理意义是什么?为何画出 T W C 的平移
   部分就得到了机器人的轨迹?
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 部分。

答：1.机器人的运动轨迹其旋转矩阵部分代表机器人的朝向变化，平移部分是机器人的整体移动，移动向量是连续的，
其移动部分的集合即构成了机器人的移动轨迹。Twc 代表的是机器人的相机视角观察世界坐标系,世界坐标系是不动的,在笛卡尔坐标系中就可以认为 Twc 是机器人在相对原点坐标在移动,当这个移动可视化呈现在观察者眼中,即是机器人的运动轨迹。

2.数据传入代码的方式：
参考点云地图传入方式
使用getlin分行读取按空格拆分传入数组
代码：

  ifstream file("/home/sunshine/draw_trajectory/trajectory.txt");
    if (!file)
    {
        cout << "Open file failure." << endl;
        return 1;
    }
    for (int i = 0; i<620; i++)
    {
        double data[8] = { 0 };
        for (auto& d : data)
            file >> d;
        Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
        Eigen::Vector3d t(data[1], data[2], data[3]);
        Sophus::SE3 SE3_traj(q, t);
        poses.push_back(SE3_traj);
    }

**注意：**ifstream file(绝对路径)
网上说可以放相对路径，但是一直输出 Open file failure.
当遇到这个错误可以尝试放绝对路径。例如： ifstream file("/home/sunshine/draw_trajectory/trajectory.txt");

运行工程的三个文件;
draw_trajectory.cpp

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

// 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)
    ifstream file("/home/sunshine/draw_trajectory/trajectory.txt");
    if (!file)
    {
        cout << "Open file failure." << endl;
        return 1;
    }
    for (int i = 0; i<620; i++)
    {
        double data[8] = { 0 };
        for (auto& d : data)
            file >> d;
        Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
        Eigen::Vector3d t(data[1], data[2], data[3]);
        Sophus::SE3 SE3_traj(q, t);
        poses.push_back(SE3_traj);
    }
    // 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);

        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
    }

}

CMakeLists.txt

cmake_minimum_required(VERSION 3.13)
project(draw_trajectory)

set(CMAKE_CXX_STANDARD 11)
include_directories("usr/include/eigen3")
find_package(Pangolin)
include_directories(${Pangolin_INCLUDE_DIRS})
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
add_executable(draw_trajectory draw_trajectory.cpp)
target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES} ${Sophus_LIBRARIES})

trajectory.txt

1305031526.671473 0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 1.000000000
1305031526.707547 0.002883195 -0.004662100 -0.002254304 0.011409802 0.010697415 0.002189494 0.999875307
1305031526.771481 0.013978966 -0.013082317 -0.010869596 0.043280017 0.032526672 0.003260542 0.998528004
1305031526.807455 -0.001601209 -0.011404546 -0.026841529 0.073491804 0.052071322 0.000915701 0.995935082
1305031526.871446 -0.004428456 -0.001333938 -0.042973492 0.115341254 0.070847765 -0.006601509 0.990774155
1305031526.907484 -0.006487503 -0.003464771 -0.058195263 0.135408968 0.081248961 -0.010381512 0.987398207
1305031526.939618 -0.014331216 -0.013660092 -0.078787915 0.147756621 0.091927201 -0.015138508 0.984625876
1305031526.971510 -0.018625503 -0.015494643 -0.089080200 0.165385425 0.100416750 -0.017430481 0.980948687
1305031527.007595 -0.020048823 -0.005540572 -0.097900160 0.181910872 0.106515355 -0.017926607 0.977364600
1305031527.039512 -0.023435375 -0.009148147 -0.106970325 0.195644155 0.113224901 -0.019022474 0.973931015
1305031527.071487 -0.026807848 0.004425369 -0.112307228 0.213958293 0.120352432 -0.020585811 0.969181776
1305031527.107498 -0.031070728 0.004864503 -0.118102826 0.221194029 0.125381440 -0.019031256 0.966949105
1305031527.139473 -0.030610107 0.008234076 -0.122761726 0.232777029 0.129590198 -0.020409001 0.963641405
1305031527.171540 -0.032993812 0.018031888 -0.131125793 0.240357533 0.133578792 -0.019260569 0.961256444
1305031527.207588 -0.036744710 0.020500474 -0.137645885 0.239894286 0.135950848 -0.017296137 0.961076975
1305031527.271501 -0.043445326 0.041889958 -0.136677355 0.232142463 0.137271211 -0.013112986 0.962857485
1305031527.307472 -0.046960510 0.043947175 -0.127684563 0.222602263 0.136040613 -0.011219901 0.965305805
1305031527.339636 -0.045433633 0.050494798 -0.122109100 0.214023039 0.131844014 -0.006879775 0.967865646
1305031527.371505 -0.044226125 0.053426970 -0.117887422 0.204703599 0.130104840 -0.003915860 0.970130801
1305031527.407412 -0.040434089 0.058478922 -0.110622235 0.195923969 0.129179820 -0.000323755 0.972073197
1305031527.439476 -0.040064313 0.059549667 -0.107858606 0.184903756 0.129534349 0.003571141 0.974175930
1305031527.471497 -0.040355999 0.060254186 -0.104310095 0.174872562 0.129172936 0.005462089 0.976065636
1305031527.507487 -0.039378427 0.065345109 -0.099110007 0.165955782 0.128173202 0.006275139 0.977747917
1305031527.539741 -0.040670216 0.068688609 -0.094280347 0.155639857 0.128989607 0.007390451 0.979327977
1305031527.571506 -0.042678047 0.073601574 -0.091266952 0.142875060 0.129394680 0.008747170 0.981207013
1305031527.607559 -0.043199223 0.075706884 -0.086145602 0.128317028 0.129251137 0.009227687 0.983231306
1305031527.671547 -0.059779059 0.086170383 -0.080027580 0.096256085 0.124875024 0.008046621 0.987459481
1305031527.707520 -0.062977917 0.080318302 -0.075421400 0.081575632 0.118124433 0.010604718 0.989585578
1305031527.771461 -0.063968100 0.079457901 -0.070676021 0.062909685 0.104486309 0.013305012 0.992445469
1305031527.807581 -0.067149058 0.071368024 -0.072235338 0.050288275 0.096843056 0.014775201 0.993918598
1305031527.839601 -0.084145665 0.083385780 -0.065109350 0.049927417 0.091642611 0.016088929 0.994409442
1305031527.871464 -0.094857678 0.090055093 -0.066003457 0.044405133 0.080871582 0.012684624 0.995654106
1305031527.907477 -0.099260926 0.085766435 -0.058261685 0.034565847 0.067202255 0.009141340 0.997098565
1305031527.939566 -0.114897557 0.077028573 -0.058029801 0.028592139 0.054828048 0.000756203 0.998086035
1305031527.971502 -0.125279635 0.082915515 -0.061315395 0.033042073 0.038314771 -0.013166402 0.998632491
1305031528.039560 -0.149975643 0.084671959 -0.052950244 0.036809143 0.004435565 -0.046019781 0.998252273
1305031528.071546 -0.160694852 0.087861642 -0.055381063 0.043458812 -0.012107812 -0.061667852 0.997076631
1305031528.107513 -0.176289976 0.090757042 -0.055422220 0.049189046 -0.027869513 -0.077940412 0.995353699
1305031528.139513 -0.182451800 0.096339859 -0.055684097 0.055754602 -0.047568955 -0.096469797 0.992633939
1305031528.171523 -0.189394832 0.097757049 -0.056705259 0.059309058 -0.062220436 -0.113738760 0.989785075
1305031528.207527 -0.202431262 0.095546886 -0.061502121 0.064778626 -0.071393289 -0.133537352 0.986344039
1305031528.239493 -0.211042851 0.098279119 -0.066208452 0.074096076 -0.080469146 -0.148469865 0.982848525
1305031528.275450 -0.223710716 0.104189500 -0.073275700 0.081444807 -0.093165532 -0.159726724 0.979374468
1305031528.307665 -0.244773716 0.107162125 -0.078257680 0.089617550 -0.112386964 -0.174391136 0.974128127
1305031528.339593 -0.267502815 0.119377658 -0.079869874 0.100359902 -0.131352678 -0.185550600 0.968630672
1305031528.375435 -0.277227014 0.122483246 -0.086401798 0.105153799 -0.156168520 -0.190717027 0.963421524
1305031528.407459 -0.286472470 0.125400484 -0.096434973 0.109714307 -0.185297355 -0.197440162 0.956370771
1305031528.439469 -0.294514090 0.129935056 -0.100482926 0.113646500 -0.217375934 -0.202731520 0.948014855
1305031528.475342 -0.309931368 0.129882887 -0.129739463 0.111446232 -0.240962654 -0.210141197 0.940934300
1305031528.539626 -0.329695433 0.143418118 -0.155989200 0.111015044 -0.298002064 -0.219036371 0.922438860
1305031528.575449 -0.329237044 0.149565578 -0.154559985 0.106090672 -0.328091890 -0.223088816 0.911773980
1305031528.607841 -0.340293765 0.151535481 -0.160942093 0.099514537 -0.350554526 -0.228903219 0.902669191
1305031528.639487 -0.340425193 0.165493459 -0.169357076 0.096142113 -0.372110546 -0.231576025 0.893679440
1305031528.707447 -0.339044899 0.179300487 -0.177713454 0.078025557 -0.396103621 -0.245452717 0.881343842
1305031528.739609 -0.342159748 0.189583004 -0.177449271 0.070908576 -0.403506339 -0.253377467 0.876330137
1305031528.775443 -0.341898620 0.206203520 -0.174872875 0.064870313 -0.407845527 -0.261114776 0.872509539
1305031528.807559 -0.348404497 0.222904056 -0.195781291 0.056483749 -0.403348863 -0.271214008 0.872102201
1305031528.839572 -0.353144258 0.236203671 -0.200213343 0.045424741 -0.405389339 -0.278263241 0.869577825
1305031528.875433 -0.357103825 0.256649941 -0.206021339 0.036655750 -0.406093627 -0.286254942 0.867065430
1305031528.907519 -0.363840312 0.273261279 -0.213352874 0.023924233 -0.403293431 -0.293037355 0.866551280
1305031528.939602 -0.373810410 0.280675173 -0.220558763 0.005930781 -0.399948001 -0.299453437 0.866218269
1305031528.975465 -0.383288622 0.293885916 -0.225069121 -0.007588292 -0.398547530 -0.297109455 0.867656767
1305031529.007487 -0.387591243 0.312122375 -0.228814498 -0.022781339 -0.395879656 -0.293224841 0.869930744
1305031529.039494 -0.396930993 0.326887786 -0.233368337 -0.039953087 -0.387588739 -0.290734917 0.873871803
1305031529.075422 -0.409061730 0.334867179 -0.235107094 -0.057665374 -0.375858516 -0.282847494 0.880569339
1305031529.107523 -0.420381188 0.348994136 -0.244129315 -0.070332937 -0.359931797 -0.270585924 0.890104294
1305031529.139597 -0.432475984 0.367254645 -0.253846556 -0.084366389 -0.344622523 -0.252671927 0.900152504
1305031529.175411 -0.442705750 0.384032130 -0.264635384 -0.099091217 -0.330286026 -0.234458074 0.908912241
1305031529.207466 -0.461968184 0.417857319 -0.283306688 -0.107409544 -0.308710963 -0.220306739 0.919035196
1305031529.239515 -0.474564463 0.434241831 -0.290974945 -0.122858316 -0.295257181 -0.214269757 0.922939599
1305031529.275389 -0.488497406 0.454518944 -0.299479723 -0.134992152 -0.277170092 -0.211710542 0.927433312
1305031529.307413 -0.505657196 0.462695599 -0.306875080 -0.152041495 -0.259664267 -0.206709743 0.930982769
1305031529.339486 -0.530183196 0.486928225 -0.326155573 -0.160189480 -0.237766638 -0.205552071 0.935710788
1305031529.375399 -0.554967403 0.482978404 -0.331514835 -0.181723729 -0.229372948 -0.205984905 0.933774471
1305031529.407513 -0.573170364 0.494490802 -0.336653709 -0.182107240 -0.230446473 -0.207627431 0.933071375
1305031529.439502 -0.592521727 0.509317398 -0.348569959 -0.176561520 -0.225443274 -0.212392747 0.934286177
1305031529.475403 -0.613592625 0.520123661 -0.360875040 -0.172136515 -0.218513519 -0.214253858 0.936331213
1305031529.507485 -0.633599639 0.528568029 -0.369254827 -0.166303858 -0.213266820 -0.219033107 0.937488556
1305031529.539489 -0.651074648 0.528935075 -0.378143847 -0.160415858 -0.206384078 -0.221350908 0.939508438
1305031529.607479 -0.686017632 0.545031309 -0.394158632 -0.154660717 -0.191093341 -0.215704605 0.945005238
1305031529.639746 -0.701905608 0.552717209 -0.398970723 -0.154912621 -0.184921563 -0.209788874 0.947520316
1305031529.675618 -0.719306707 0.559796870 -0.401483953 -0.157197714 -0.176389754 -0.200863779 0.950699389
1305031529.707557 -0.735418081 0.563297987 -0.403395295 -0.162735164 -0.169739127 -0.191895753 0.952828407
1305031529.739568