SLAM 十四讲之第三讲

SLAM 十四讲—第三讲

  这边博客主要介绍一些我个人在调试第三讲的程序过程中遇到的一些问题。

问题一

  从GitHub上将相关代码下载下来后用KDevelop软件进行调试，这个问题出现在build过程中，是由于缺少对C++标准的说明，因此在CMakeLists中添加上引用说明即可。如下：

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

重新在KDevelop中运行build即可。
修改后的CMakeLists.txt文件

include_directories("/usr/include/eigen3")
add_executable(coordinateTransform coordinateTransform.cpp)

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

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(plotTrajectory plotTrajectory.cpp)
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})

问题二

这个问题主要参考下面这个博主的解决方法：解决方法
完成后，记得重新编译安装pangolin，不然调试的时候还会出现之前的问题。

最后结果

调试代码

下面是plotTrajectory.cpp文件

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>

using namespace std;
using namespace Eigen;

string trajectory_file = "/home/sqs/trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char** argv) {
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
    ifstream fin(trajectory_file);
    if (!fin) {
        cout << "cannot find trajectory file at  " <<trajectory_file << endl;
        return 1;
    }

    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
    }
    cout << "read total " << poses.size() << " pose entries" << endl;
    DrawTrajectory(poses);
    return 0;
}

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
    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, 0.0, 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(); i++) {
             Vector3d Ow = poses[i].translation();
             Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
             Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
             Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
             glBegin(GL_LINES);
             glColor3f(1.0, 0, 0);
             glVertex3d(Ow[0], Ow[1], Ow[2]);
             glVertex3d(Xw[0], Xw[1], Xw[2]);
             glColor3f(0.0, 1.0, 0.0);
             glVertex3d(Ow[0], Ow[1], Ow[2]);
             glVertex3d(Yw[0], Yw[1], Yw[2]);
             glColor3f(0.0, 0.0, 1.0);
             glVertex3d(Ow[0], Ow[1], Ow[2]);
             glVertex3d(Zw[0], Zw[1], Zw[2]);
             glEnd();
         }

         for (size_t i = 0; i < poses.size(); i++) {
             glColor3f(0.0, 0.0, 0.0);
             glBegin(GL_LINE);
             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);
     }
}

致谢

感谢Warship博主。
大家在调试过程中遇到其他问题可以在评论区一起讨论一下，感谢感谢（✿✿ヽ(°▽°)ノ✿）