视觉SLAM实践入门——（3）运动的可视化演示

旋转矩阵等数学表示方式不直观，考虑将相机的运动轨迹画到一个窗口中。

假设轨迹文件已经存储在trajectory.txt，每一行用下面格式存储：

time，tx，ty，tz，qx，qy，qz，qw，

其中time是位姿记录时间，t是平移，q是旋转四元数（以机器人坐标系到世界坐标系的记录Twr）

在画轨迹时，把轨迹画成一系列点组成的序列。这些点的坐标，其实是机器人坐标系的原点在世界坐标系中的坐标。考虑机器人坐标系的原点Or，这个原点在世界坐标系中的坐标Ow

Ow = Twr * Or = twr，其中Twr表示从机器人坐标系到世界坐标系的变换

其结果就是Twr的平移部分（平移向量twr）。因此可以从Twr直观看到相机在哪里（所以我们说Twr比Trw更直观）。因为这个原因，可视化程序里，轨迹文件存储Twr而不是Trw。

 

通过以下命令获取资源包，在slambook2/ch3/examples/中有轨迹文件trajectory.txt，将它放在工程build目录下（可执行程序所在目录）

git clone https://github.com/gaoxiang12/slambook2.git

如果下载缓慢，参考https://blog.csdn.net/floatinglong/article/details/116033425

 

安装3D绘图程序库

MATLAB、Python的Matplotlib、OpenGL等都支持3D绘图。在Linux中，常见的库是基于OpenGL的Pangolin库，它在支持OpenGL的绘图操作基础上还提供一些GUI功能。下面通过git（《SLAM十四讲》作者的主页）安装这个库：

安装git

sudo apt install git

安装glew

sudo apt install libglew-dev

从git获取Pangolin

git clone https://github.com/stevenlovegrove/Pangolin.git

安装Pangolin

cd Pangolin
mkdir build
cd build
cmake ..
cmake --build .
sudo make install

 

安装后可能会出现C++标准和链接库的问题，解决办法参考https://blog.csdn.net/floatinglong/article/details/116033546

 

 

实例代码

 

#include <iostream>
#include <unistd.h>
#include <pangolin/pangolin.h>

using namespace std;
using namespace Eigen;

#define TRAJECTORY_FILE "./trajectory.txt"

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses)
{
  //创建一个窗口，绑定属性：窗口名Trajectory Viewer，长1024，宽768
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  
  pangolin::OpenGlRenderState sCam(
    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 &dCam = pangolin::CreateDisplay()
  .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
  .SetHandler(new pangolin::Handler3D(sCam));
  
  
  while(pangolin::ShouldQuit() == false)
  {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    dCam.Activate(sCam);
    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, 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_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();
    //sleep5ms
    usleep(5000);	
  }
}
int main(int argc, char **argv) {  
  
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  
  ifstream fin(TRAJECTORY_FILE);
  if(!fin)
  {
    cout << "打开" << 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;
}

                                                                                                                                                                                                      

pangolin参考教程和手册

https://blog.csdn.net/weixin_43991178/article/details/105119610

http://docs.ros.org/en/fuerte/api/pangolin_wrapper/html/index.html