Pangolin库的使用

使用Pangolin画出相机的轨迹（包括朝向）。

数据集结构data.csv：

 1 #timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], v_RS_R_y [m s^-1], v_RS_R_z [m s^-1], b_w_RS_S_x [rad s^-1], b_w_RS_S_y [rad s^-1], b_w_RS_S_z [rad s^-1], b_a_RS_S_x [m s^-2], b_a_RS_S_y [m s^-2], b_a_RS_S_z [m s^-2]
 2 1403636580838555648,4.688319,-1.786938,0.783338,0.534108,-0.153029,-0.827383,-0.082152,-0.027876,0.033207,0.800006,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
 3 1403636580843555328,4.688177,-1.786770,0.787350,0.534640,-0.152990,-0.826976,-0.082863,-0.029272,0.033992,0.804771,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
 4 1403636580848555520,4.688028,-1.786598,0.791382,0.535178,-0.152945,-0.826562,-0.083605,-0.030043,0.034999,0.808240,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
 5 1403636580853555456,4.687878,-1.786421,0.795429,0.535715,-0.152884,-0.826146,-0.084391,-0.030230,0.035853,0.810462,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
 6 1403636580858555648,4.687727,-1.786240,0.799484,0.536244,-0.152821,-0.825731,-0.085213,-0.029905,0.036316,0.811406,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
 7 1403636580863555328,4.687579,-1.786059,0.803540,0.536768,-0.152768,-0.825314,-0.086049,-0.029255,0.036089,0.811225,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
 8 1403636580868555520,4.687435,-1.785881,0.807594,0.537289,-0.152725,-0.824896,-0.086890,-0.028469,0.035167,0.810357,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
 9 1403636580873555456,4.687295,-1.785709,0.811642,0.537804,-0.152680,-0.824481,-0.087725,-0.027620,0.033777,0.808910,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
10 1403636580878555648,4.687158,-1.785544,0.815682,0.538317,-0.152627,-0.824067,-0.088553,-0.026953,0.031990,0.806951,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593

前八项分别为时间戳，x，y，z，$q_0,q_1,q_2,q_3$。

  1 #include <iostream>
  2 #include <pangolin/pangolin.h>
  3 #include <Eigen/Core>
  4 #include <sophus/se3.h>
  5 using namespace std;
  6 
  7 typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> VecSE3;
  8 typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVec3d;
  9 
 10 string file = "./data.csv";
 11 
 12 void Draw(const VecSE3 &poses);
 13 int main(int argc, char **argv)
 14 {
 15     //读位姿
 16     VecSE3 poses;
 17     VecVec3d points;
 18     ifstream fin(file);//位姿
 19 
 20     string lineStr;
 21     int j = 0;
 22     while(getline(fin,lineStr))//每行
 23     {
 24         j+=1;//隔100个点取一个数据
 25         if (j%100 != 0 )
 26             continue;
 27 
 28         //cout<<lineStr <<endl;
 29         stringstream ss(lineStr);
 30         string str;
 31         double data[8];
 32         int i = 0;
 33         while (getline(ss, str, ','))
 34         {
 35             stringstream intg(str);
 36             if (i >= 8)//只取前八个
 37                 continue;
 38             intg >> data[i];
 39             i+=1;
 40         }
 41         poses.push_back(Sophus::SE3(
 42             //eigen.tuxfamily.org/dox-devel/classEigen_1_1Quaternion.html
 43                 Eigen::Quaterniond(data[4], data[5], data[6], data[7]),//四元数
 44                 Eigen::Vector3d(data[1], data[2], data[3])//位移
 45         ));
 46         //cout << data[1] <<" "<< data[2]<<" "<< data[3]<<" "<< data[4]<<endl;//位移
 47         if (!fin.good()) break;
 48 
 49     }
 50     fin.close();
 51 
 52     Draw(poses);
 53     return 0;
 54 
 55 }
 56 
 57 void Draw(const VecSE3 &poses) 
 58 {
 59     if (poses.empty() ) 
 60     {
 61         cerr << "parameter is empty!" << endl;
 62         return;
 63     }
 64     // create pangolin window and plot the trajectory
 65     pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
 66     glEnable(GL_DEPTH_TEST);//深度测试
 67     glEnable(GL_BLEND);
 68     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 69     pangolin::OpenGlRenderState s_cam(//摆放一个相机
 70             pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
 71             pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
 72     );
 73     pangolin::View &d_cam = pangolin::CreateDisplay()//创建一个窗口
 74             .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
 75             .SetHandler(new pangolin::Handler3D(s_cam));
 76 
 77     while (pangolin::ShouldQuit() == false)
 78     {
 79         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//消除颜色缓冲
 80         d_cam.Activate(s_cam);
 81 
 82         glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
 83         // draw poses
 84         //画相机朝向
 85         for (auto &Twc: poses)//从poses中取位姿
 86         {
 87             glPushMatrix();
 88             Sophus::Matrix4f m = Twc.matrix().cast<float>();
 89             glMultMatrixf((GLfloat *) m.data());
 90 
 91             const float w = 0.25;
 92             const float h = w*0.75;
 93             const float z = w*0.6;
 94             glColor3f(1, 0, 0);
 95             glLineWidth(2);
 96             glBegin(GL_LINES);
 97             //画相机模型
 98             glVertex3f(0, 0, 0);
 99             glVertex3f(w,h,z);
100             glVertex3f(0, 0, 0);
101             glVertex3f(w,-h,z);
102             glVertex3f(0, 0, 0);
103             glVertex3f(-w,-h,z);
104             glVertex3f(0, 0, 0);
105             glVertex3f(-w,h,z);
106             glVertex3f(w,h,z);
107             glVertex3f(w,-h,z);
108             glVertex3f(-w,h,z);
109             glVertex3f(-w,-h,z);
110             glVertex3f(-w,h,z);
111             glVertex3f(w,h,z);
112             glVertex3f(-w,-h,z);
113             glVertex3f(w,-h,z);
114 
115             glEnd();
116             glPopMatrix();
117         }
118         //画轨迹
119         glLineWidth(2);
120         for (size_t i = 0; i < poses.size() - 1; i++)
121         {
122             glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
123             glBegin(GL_LINES);
124             auto p1 = poses[i], p2 = poses[i + 1];
125             glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
126             glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
127         }
128         glEnd();
129         pangolin::FinishFrame();
130         usleep(5000);   // sleep 5 ms 
131     }
132 }

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )  
project( show )  
      
set( CMAKE_BUILD_TYPE "Release" )  
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )  
      
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )  
      
    # 寻找G2O Cholmod eigen3  
find_package( G2O REQUIRED )  
find_package( Cholmod )  
include_directories(   
     ${G2O_INCLUDE_DIRS} ${CHOLMOD_INCLUDE_DIR}  
     "/usr/include/eigen3"  
)  
      
    # sophus  
find_package( Sophus REQUIRED )  
include_directories( ${Sophus_INCLUDE_DIRS} )

find_package( Pangolin REQUIRED)
include_directories( ${Pangolin_INCLUDE_DIRS} )
      
add_executable( draw draw.cpp )  
      
target_link_libraries( draw      
    ${CHOLMOD_LIBRARIES}  
    ${Sophus_LIBRARIES}  
    ${Pangolin_LIBRARIES}
)

除此之外，还需添加cmake_modules。

运行结果：