高博课程第三次作业：轨迹的误差

7 * 轨迹的误差

在根目录下新建了build文件 把数据文件放入其中  ，编写了估计误差的ground_error.cpp文件和CMakeLists.txt 文件，代码如下

//ground_error.cpp writed by zhang ning 2018/3/15
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>

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

using namespace std;



// 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("../data/trajectory.txt");
    if(!infile) cout<<"error"<<endl;
    
    cout<<"存入数组"<<endl;   //先将文件中的数据存入到一个二维数组中
    double data;
    double a[620][8];
    double *p=&a[0][0];
    while(infile>>data)             //遇到空白符结束
    {
        *p=data;
         p++;
    }
    infile.close();
    for(int i=0;i<620;i++)
    {   for(int j=0;j<8;j++)
           cout<<a[i][j]<<"	";
         cout<<endl;
    }
    for(int i=0;i<620;i++)   //分别对每一行数据生成一个变换矩阵，然后存入动态数组poses中
    {    
    Eigen::Quaterniond q = Eigen::Quaterniond(a[i][7],a[i][4],a[i][5],a[i][6]);
        Eigen::Vector3d t;
    t<<a[i][1],a[i][2],a[i][3];
        Sophus::SE3 SE3_qt(q,t);
        //cout<<SE3_qt<<endl;
        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
    }

}

# writed by zhang ning 2018/3/15
cmake_minimum_required( VERSION 2.8 )

project(trajectory.txt)

set( CMAKE_BUILD_TYPE "Debug" )

set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) 



find_package( Sophus REQUIRED)
find_package( Pangolin REQUIRED)


include_directories( "/usr/include/eigen3" )
include_directories( ${Sophus_INCLUDE_DIRS} )
include_directories( ${Pangolin_INCLUDE_DIRS} )

add_executable( draw_trajectory draw_trajectory.cpp)

target_link_libraries( draw_trajectory ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES} )

add_executable( ground_error ground_error.cpp)
target_link_libraries( ground_error ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES})

运行截图如下，与参考答案一致