视觉SLAM十四讲课后习题之三：李群和李代数

• 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 infile;
    // infile.open( trajectory_file );
    // if(!infile)
    // cout << "error: no file!"<<endl;
    fstream fin( trajectory_file );
    string line;
    double t, tx, ty, tz, qx, qy, qz, qw;
    while( getline( fin, line ) )
    {
        stringstream lineStrem( line );
        lineStrem >> 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);

        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

cmake_minimum_required( VERSION 2.8 )
project( trajectShow )
SET(CMAKE_CXX_FLAGS "-std=c++0x")

# 为使用 sophus，您需要使用find_package命令找到它
find_package( Sophus REQUIRED )
find_package( Pangolin REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} 
                     ${Pangolin_INCLUDE_DIRS})

add_executable( trajectShow draw_trajectory.cpp )
target_link_libraries( trajectShow ${Sophus_LIBRARIES} 
                       ${Pangolin_LIBRARIES} )

• 效果