视觉slam十四讲 pdf_视觉SLAM十四讲——第三章 李群与李代数 课后作业&手推...

首先呢，放一下整理好的PDF文件链接，供大家下载！

链接:

百度网盘链接​pan.baidu.com

密码: fe4u

然后是每张图片格式的展示，也方便在知乎平台观看：

绘出轨迹的程序源码 draw_trajectory.cpp：（知乎的语言代码块类型还是很多的~）

#include <unistd.h>
#include <string>
#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <sophus/se3.hpp>

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

using namespace std;

// path to trajectory file
string trajectory_file = "/home/fengyixiao/Dark_Blue/SLAM/course/Complete/PA3/trajectory.txt";

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

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

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

    /// implement pose reading code
    // start your code here (5~10 lines)
    ifstream f_str(trajectory_file);

    if (!f_str)
    {
        cout << "cannot find the file of trajectory at: " << trajectory_file << endl;
        return 1;
    }

    while(!f_str.eof()) //end of file
    {
        double time, tx, ty, tz, qx, qy, qz, w;
        f_str >> time >> tx >> ty >> tz >> qx >> qy >> qz >> w;
        Eigen::Quaterniond q= Eigen::Quaterniond(w, qx, qy, qz).normalized();
        Eigen::Vector3d t(tx, ty, tz);
        Sophus::SE3d SE3_qt(q, t);
        poses.push_back(SE3_qt);
    }
    cout << "complete it! read total: " << poses.size() << endl;
    // end your code here

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

void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> 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()) {
        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
    }

}

vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>>才是标准的定义容器方法，只是我们一般情况下定义容器的元素都是C++中的类型，所以可以省略，这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。