【SLAM】视觉SLAM十四讲（三：李群与李代数）

参考https://blog.csdn.net/weixin_41074793/article/details/84853291

轨迹描绘

1.事实上,T W C 的平移部分即构成了机器人的轨迹。它的物理意义是什么?为何画出 T W C 的平移
部分就得到了机器人的轨迹?
坐标的变换就是Twc的平移部分，我理解的物理意义就是机器人从某一坐标移到了另一坐标，因此平移部分就是机器人的轨迹。
2. 我为你准备了一个轨迹文件(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 部分。

draw_trajectory.cpp

#include 
#include 
#include 
#include 
#include 
#include  
#include 
#include 
#include 

// need pangolin for plotting trajectory
#include 

using namespace std;

// path to trajectory file
string trajectory_file = "/home/huangyuan/slam/slambook/pa3/drawtrace/trajectory.txt";

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

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

    vector> poses;

    /// implement pose reading code
    // start your code here (5~10 lines)
    //读取txt文件
     // implement pose reading code
    ifstream infile(trajectory_file);
    double t1,tx,ty,tz,qx,qy,qz,qw;
    string line;
    if(infile)
    {
        while(getline(infile,line))
        {
            stringstream record(line);    //从string读取数据
            record>>t1>>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);
        }
    }
    else
   {
       cout<<"没找到这个文件"<> 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

PROJECT(drawtrace)

# Check C++11 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
   add_definitions(-DCOMPILEDWITHC11)
   message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
   add_definitions(-DCOMPILEDWITHC0X)
   message(STATUS "Using flag -std=c++0x.")
else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

find_package(OpenCV 3.0 QUIET)
if(NOT OpenCV_FOUND)
   find_package(OpenCV 2.4.3 QUIET)
   if(NOT OpenCV_FOUND)
      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
   endif()
endif()


find_package(Sophus)
include_directories( ${Sophus_INCLUDE_DIRS} )

# eigen
include_directories( "/usr/include/eigen3/" )


find_package(Pangolin)
include_directories( ${Pangolin_INCLUDE_DIRS} )


ADD_EXECUTABLE(drawtrace draw_trajectory.cpp)
target_link_libraries(${PROJECT_NAME}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${Sophus_LIBRARIES}
)

轨迹误差

CMakeLists.txt

PROJECT(compare)

# Check C++11 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
   add_definitions(-DCOMPILEDWITHC11)
   message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
   add_definitions(-DCOMPILEDWITHC0X)
   message(STATUS "Using flag -std=c++0x.")
else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

find_package(OpenCV 3.0 QUIET)
if(NOT OpenCV_FOUND)
   find_package(OpenCV 2.4.3 QUIET)
   if(NOT OpenCV_FOUND)
      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
   endif()
endif()

# sophus
find_package(Sophus)
include_directories( ${Sophus_INCLUDE_DIRS} )

# eigen
include_directories( "/usr/include/eigen3/" )

#Pangolin
find_package(Pangolin)
include_directories( ${Pangolin_INCLUDE_DIRS} )

# exe
ADD_EXECUTABLE(compare compare.cpp)
target_link_libraries(${PROJECT_NAME}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${Sophus_LIBRARIES}
)

compare.cpp

#include 
#include 
#include 
#include 
#include 
#include  
#include 
#include 
#include 

// need pangolin for plotting trajectory
#include 

using namespace std;

// path to trajectory file
string estimated_file = "/home/huangyuan/slam/slambook/pa3/drawtrace/estimated.txt";
string groundtruth_file = "/home/huangyuan/slam/slambook/pa3/drawtrace/groundtruth.txt";
vector> pose1,pose2;

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector> pose1,vector> pose2);
void readData(string filepath);
void ErrorTrajectory();

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

    

    /// implement pose reading code
    // start your code here (5~10 lines)
    readData(estimated_file);
    readData(groundtruth_file);
    ErrorTrajectory();
    // end your code here

    // draw trajectory in pangolin
    DrawTrajectory(pose1,pose2);
    return 0;
}

/*******************************************************************************************/
void ErrorTrajectory()
{
    double RMSE = 0;
    Eigen::Matrix se3;
    vector error;
    for(int i=0;i> poses;
    ifstream infile(filepath);
    double t1,tx,ty,tz,qx,qy,qz,qw;
    string line;
    if(infile)
    {
        while(getline(infile,line))
        {
            stringstream record(line);    //从string读取数据
            record>>t1>>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);
        }
    }
    else
   {
       cout<<"没找到这个文件"<> pose1,vector> pose2) {
    if (pose1.empty()) {
        cerr << "estimated is empty!" << endl;
        return;
    } 
    if(pose2.empty()){
        cerr << "groundtruth 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 < pose1.size() - 1; i++) {
            glColor3f(1 - (float) i / pose1.size(), 0.0f, (float) i / pose1.size());
            glBegin(GL_LINES);
            auto p1 = pose1[i], p2 = pose1[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 < pose2.size() - 1; i++) {
            glColor3f(1 - (float) i / pose2.size(), 0.0f, (float) i / pose2.size());
            glBegin(GL_LINES);
            auto p1 = pose2[i], p2 = pose2[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
    }

}