Pangolin库的使用

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

数据集结构data.csv：

 #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]
 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
 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
 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
 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
 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
 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
 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
 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
 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$。

 #include <iostream>
 #include <pangolin/pangolin.h>
 #include <Eigen/Core>
 #include <sophus/se3.h>
 using namespace std;

 typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> VecSE3;
 typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVec3d;

 string file = "./data.csv";

 void Draw(const VecSE3 &poses);
 int main(int argc, char **argv)
 {
     //读位姿
     VecSE3 poses;
     VecVec3d points;
     ifstream fin(file);//位姿

     string lineStr;
     int j = ;
     while(getline(fin,lineStr))//每行
     {
         j+=;//隔100个点取一个数据
         if (j% !=  )
             continue;

         //cout<<lineStr <<endl;
         stringstream ss(lineStr);
         string str;
         double data[];
         int i = ;
         while (getline(ss, str, ','))
         {
             stringstream intg(str);
             if (i >= )//只取前八个
                 continue;
             intg >> data[i];
             i+=;
         }
         poses.push_back(Sophus::SE3(
             //eigen.tuxfamily.org/dox-devel/classEigen_1_1Quaternion.html
                 Eigen::Quaterniond(data[], data[], data[], data[]),//四元数
                 Eigen::Vector3d(data[], data[], data[])//位移
         ));
         //cout << data[1] <<" "<< data[2]<<" "<< data[3]<<" "<< data[4]<<endl;//位移
         if (!fin.good()) break;

     }
     fin.close();

     Draw(poses);
     return ;

 }

 void Draw(const VecSE3 &poses)
 {
     if (poses.empty() )
     {
         cerr << "parameter is empty!" << endl;
         return;
     }
     // create pangolin window and plot the trajectory
     pangolin::CreateWindowAndBind("Trajectory Viewer", , );
     glEnable(GL_DEPTH_TEST);//深度测试
     glEnable(GL_BLEND);
     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
     pangolin::OpenGlRenderState s_cam(//摆放一个相机
             pangolin::ProjectionMatrix(, , , , , , 0.1, ),
             pangolin::ModelViewLookAt(, -0.1, -1.8, , , , 0.0, -1.0, 0.0)
     );
     pangolin::View &d_cam = pangolin::CreateDisplay()//创建一个窗口
             .SetBounds(0.0, 1.0, pangolin::Attach::Pix(), 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(0.0f, 0.0f, 0.0f, 0.0f);
         // draw poses
         //画相机朝向
         for (auto &Twc: poses)//从poses中取位姿
         {
             glPushMatrix();
             Sophus::Matrix4f m = Twc.matrix().cast<float>();
             glMultMatrixf((GLfloat *) m.data());

             const float w = 0.25;
             const float h = w*0.75;
             const float z = w*0.6;
             glColor3f(, , );
             glLineWidth();
             glBegin(GL_LINES);
             //画相机模型
             glVertex3f(, , );
             glVertex3f(w,h,z);
             glVertex3f(, , );
             glVertex3f(w,-h,z);
             glVertex3f(, , );
             glVertex3f(-w,-h,z);
             glVertex3f(, , );
             glVertex3f(-w,h,z);
             glVertex3f(w,h,z);
             glVertex3f(w,-h,z);
             glVertex3f(-w,h,z);
             glVertex3f(-w,-h,z);
             glVertex3f(-w,h,z);
             glVertex3f(w,h,z);
             glVertex3f(-w,-h,z);
             glVertex3f(w,-h,z);

             glEnd();
             glPopMatrix();
         }
         //画轨迹
         glLineWidth();
         for (size_t i = ; i < poses.size() - ; i++)
         {
             glColor3f( - (float) i / poses.size(), 0.0f, (float) i / poses.size());
             glBegin(GL_LINES);
             auto p1 = poses[i], p2 = poses[i + ];
             glVertex3d(p1.translation()[], p1.translation()[], p1.translation()[]);
             glVertex3d(p2.translation()[], p2.translation()[], p2.translation()[]);
         }
         glEnd();
         pangolin::FinishFrame();
         usleep();   // sleep 5 ms
     }
 }

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。

运行结果：