SLAM十四讲第3章编程题

文件存放

.cpp/CMakeLists.txt/png/build都放在主文件夹里，所以执行文件和cmake ../make都在build里

---

7.轨迹的描述

 draw_trajectory.cpp

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

// 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 ifs (trajectory_file);
    if(!ifs)
    {
       cout<<"打开失败"<<endl;
       return 0;
    }
    
    while(!ifs.eof())
    {
       float time,tx,ty,tz,qx,qy,qz,qw;
       ifs >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
       Eigen::Quaterniond q(qw,qx,qy,qz);
       q.normalize();
       Eigen::Vector3d t(tx,ty,tz);
       Sophus::SE3 SE3_qt(q,t); // SE3
       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.txt

cmake_minimum_required( VERSION 2.8 )
project( draw_trajectory )

# 添加Eigen头文件
include_directories( "/usr/include" )
include_directories( "/usr/include/eigen3" )
find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)
include_directories(${Pangolin_INCLDUE_DIRES} ${Sophus_INCLDUE_DIRES})

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

---

8.轨迹的误差

estimate_error.cpp

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

#include <pangolin/pangolin.h>
#include <cassert>

using namespace std;
//T模板
// 文件路径
string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";
typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
//画图函数原型
void DrawTrajectory(const poses &gt,const poses &es);
//读取文件并求出T
poses ReadTrajectory(const string &path);

int main(int argc, char **argv) {
    //画图  
    poses estimated=ReadTrajectory(estimated_file);
    poses groundtruth=ReadTrajectory(groundtruth_file);
    DrawTrajectory(estimated,groundtruth);
    //判断文件
    assert(!estimated.empty()&& !groundtruth.empty());
    assert(estimated.size()==groundtruth.size());
    //求RMSE
    int num=estimated.size();
    double RMSE = 0;
    for(int i =0;i<num;i++){
       Sophus::SE3 p1=groundtruth[i],p2 = estimated[i];
       double error =(p1.inverse()*p2).log().norm();
       RMSE += error*error;
    }
    RMSE = sqrt(RMSE/num);
    cout << "The RMSE is :"<< RMSE << endl;  
    return 0;
}

/*******************************************************************************************/
void DrawTrajectory(const poses &gt,const poses &es) {
    if (gt.empty()||gt.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 < gt.size() - 1; i++) {
            glColor3f(1 - (float) i / gt.size(), 0.0f, (float) i / gt.size());
            glBegin(GL_LINES);
            auto p1 = gt[i], p2 = gt[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        for (size_t i = 0; i < es.size() - 1; i++) {
            glColor3f(1 - (float) i / es.size(), 0.0f, (float) i / es.size());
            glBegin(GL_LINES);
            auto p1 = es[i], p2 = es[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
    }
}

/*******************************************************************************************/
poses ReadTrajectory(const string &path) {
    poses trajectory;
    ifstream ifs (path);
    if(!ifs){
       cout<<"打开失败"<<endl;
    }
    while(!ifs.eof()){
      double time,tx,ty,tz,qx,qy,qz,qw;
      ifs >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
      Eigen::Quaterniond q(qw,qx,qy,qz);
      Eigen::Vector3d t(tx,ty,tz);
      Sophus::SE3 SE3_qt(q,t); 
      trajectory.push_back(SE3_qt);
    }
    return trajectory;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( estimate_error )

# 添加Eigen头文件
include_directories( "/usr/include" )
include_directories( "/usr/include/eigen3" )
find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)
include_directories(${Pangolin_INCLDUE_DIRES} ${Sophus_INCLDUE_DIRES})

add_executable(RMSE estimate_error.cpp )
target_link_libraries(RMSE ${Pangolin_LIBRARIES})
target_link_libraries(RMSE ${Sophus_LIBRARIES})