高博SLAM基础课第三次作业——轨迹绘图计算误差

作业描述

我为你准备了一个轨迹文件(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 部分。

遇到的坑：

　　代码文件放进clion编译不出来，include后也发现各种未定义引用，后来发现要改Cmakelist.txt才行

　　照着ORB-SLAM2的Cmake写法，把eigen3和Pangolin库都target_link进来，报的未定义引用只剩了Sophus库，再用find_package时候发现不行，网上查到是因为Sophus的lib文件在find_package时候不能引入路径。于是手动

　　set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so")即可

　　在绘图时候发现轨迹的一端经常拉出一条长直线，推测是vector最后一项的值有问题，把绘图循环里的末尾值减少一个，轨迹正常。

Cmakelist.txt如下：

cmake_minimum_required(VERSION 2.8)
project(useEigen)
find_package(Pangolin REQUIRED)
find_package(Eigen3 3.1.0 REQUIRED)

set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so")
set(CMAKE_BUILD_TYPE "Release")
SET( CMAKE_CXX_FLAGS "-std=c++11 -O3")

include_directories(
        ${Pangolin_INCLUDE_DIRS}
        ${EIGEN3_INCLUDE_DIR}
        )

add_executable(eigenMatrix eigenMatrix.cpp)

add_executable(tra trajectory.cpp)

target_link_libraries(tra
        ${EIGEN3_LIBS}
        ${Pangolin_LIBRARIES}
        ${Sophus_LIBRARIES}
        )

源码如下：

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
// path to trajectory file
string trajectory_file = "/home/steve/L3/code/groundtruth.txt";
string estimated_file = "/home/steve/L3/code/estimated.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> > ,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3> > );

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

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3> > poses1;
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3> > poses2;
    /// implement pose reading code
    // start your code here (5~10 lines)
    ifstream f(trajectory_file);
    string s0;
    while(!f.eof()) {
        Vector3d t_tmp;
        double db1, db2, db3,db4;
        f>> db1 >> db1 >> db2 >> db3;
        t_tmp << db1, db2, db3;
        f >> db1 >> db2 >> db3>>db4;
        Quaterniond q(db4,db1,db2,db3);
        //cout<<q<<endl;
        poses1.emplace_back(Sophus::SE3(q,t_tmp));
    }

    ifstream g(estimated_file);

    while(!g.eof()) {
        Vector3d t_tmp;
        double db1, db2, db3,db4;
        g>> db1 >> db1 >> db2 >> db3;
        t_tmp << db1, db2, db3;
        g >> db1 >> db2 >> db3>>db4;
        Quaterniond q(db4,db1,db2,db3);
        //cout<<q<<endl;
        poses2.emplace_back(Sophus::SE3(q,t_tmp));
    }
    // end your code here

   //  draw trajectory in pangolin
    DrawTrajectory(poses1,poses2);

    return 0;



}

/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3> > poses1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3> > poses2){
    if (poses1.empty()&&poses1.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 < poses1.size() - 2; i++) {
            glColor3f(1 - (float) i / poses1.size(), 0.0f, (float) i / poses1.size());
            glBegin(GL_LINES);
            auto p1 = poses1[i], p2 = poses1[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 < poses2.size() - 2; i++) {
            glColor3f(1 - (float) i / poses2.size(), 0.0f, (float) i / poses2.size());
            glBegin(GL_LINES);
            auto p3 = poses2[i], p4 = poses2[i + 1];
            glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
            glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
            glEnd();
        }



        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

学习另一博客把绘图部分输入改为两个使得可以绘出两条轨迹比较。对estimated和groundtruth做了对比，绘图如下：

一开始以为是3条线后来发现是根据长度变颜色。。。

光滑的是基准，粗糙的是估计值（应该还没有做后端优化）

下一题:计算误差值

阅读se3各接口后调用代码如下

 double error_s=0;
    for(int i=0;i<poses1.size()-1;i++){//也是要-1不然结果不对
        error_s+= Sophus::SE3::log(poses1[i].inverse()*poses2[i]).squaredNorm();
    }
    error_s=sqrt(error_s/(poses1.size()-1));

    cout<<error_s<<endl;

hat是把6维李代数变成4×4李群矩阵，vee是相反，这里log是对数映射，自动就变成了6维李代数，不需要调用vee或者hat

调用结果：

PS：

Eigen::Isometry3d T; 是SE(3)的eigen写法

初始化为调用T.pretranslate(t)可以输入t

　　　　　　T.prerotate(R)输入R

或者用Isometry3d T(q)用四元数初始化R

使用时用T.matrix()