使用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。
运行结果：
