SLAM 显示相机的3D位姿

这个程序可以显示相机的3D位姿测试物体是一个立方体。左边会实时显示相机位姿对应的旋转矩阵、平移、欧拉角和四元数。右边是鼠标操作区共有四种操作模式。红线是X轴绿线是Y轴蓝线是Z轴。1.鼠标左键移动的是相机只有第二行t的数据在变化2.鼠标右键旋转立方体所有数据都在变化3.鼠标中键旋转相机只有第二行t的数据不改变其他三行数据都在变化4.鼠标左键和右键同时按下绕视线轴旋转所有数据都在变化可能我理解的也不是很到位所以就把英文说明贴出来大家可以自己看一下视频演示https://www.bilibili.com/video/BV1F59hBXEFG/代码#include iostream #include iomanip using namespace std; #include Eigen/Core #include Eigen/Geometry using namespace Eigen; #include pangolin/pangolin.h struct RotationMatrix { Matrix3d matrix Matrix3d::Identity(); }; ostream operator(ostream out, const RotationMatrix r) { out.setf(ios::fixed); Matrix3d matrix r.matrix; out ; out [ setprecision(2) matrix(0, 0) , matrix(0, 1) , matrix(0, 2) ], [ matrix(1, 0) , matrix(1, 1) , matrix(1, 2) ], [ matrix(2, 0) , matrix(2, 1) , matrix(2, 2) ]; return out; } istream operator(istream in, RotationMatrix r) { return in; } struct TranslationVector { Vector3d trans Vector3d(0, 0, 0); }; ostream operator(ostream out, const TranslationVector t) { out [ t.trans(0) , t.trans(1) , t.trans(2) ]; return out; } istream operator(istream in, TranslationVector t) { return in; } struct QuaternionDraw { Quaterniond q; }; ostream operator(ostream out, const QuaternionDraw quat) { auto c quat.q.coeffs(); out [ c[0] , c[1] , c[2] , c[3] ]; return out; } istream operator(istream in, const QuaternionDraw quat) { return in; } int main(int argc, char **argv) { pangolin::CreateWindowAndBind(visualize geometry, 1000, 600); glEnable(GL_DEPTH_TEST); pangolin::OpenGlRenderState s_cam( pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000), pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY) ); const int UI_WIDTH 500; pangolin::View d_cam pangolin::CreateDisplay(). SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f). SetHandler(new pangolin::Handler3D(s_cam)); // ui pangolin::VarRotationMatrix rotation_matrix(ui.R, RotationMatrix()); pangolin::VarTranslationVector translation_vector(ui.t, TranslationVector()); pangolin::VarTranslationVector euler_angles(ui.rpy, TranslationVector()); pangolin::VarQuaternionDraw quaternion(ui.q, QuaternionDraw()); pangolin::CreatePanel(ui).SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH)); while (!pangolin::ShouldQuit()) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); d_cam.Activate(s_cam); pangolin::OpenGlMatrix matrix s_cam.GetModelViewMatrix(); Matrixdouble, 4, 4 m matrix; RotationMatrix R; for (int i 0; i 3; i) for (int j 0; j 3; j) R.matrix(i, j) m(j, i); rotation_matrix R; TranslationVector t; t.trans Vector3d(m(0, 3), m(1, 3), m(2, 3)); t.trans -R.matrix * t.trans; translation_vector t; TranslationVector euler; euler.trans R.matrix.eulerAngles(2, 1, 0); euler_angles euler; QuaternionDraw quat; quat.q Quaterniond(R.matrix); quaternion quat; glColor3f(1.0, 1.0, 1.0); pangolin::glDrawColouredCube(); // draw the original axis glLineWidth(3); glColor3f(0.8f, 0.f, 0.f); glBegin(GL_LINES); glVertex3f(0, 0, 0); glVertex3f(10, 0, 0); glColor3f(0.f, 0.8f, 0.f); glVertex3f(0, 0, 0); glVertex3f(0, 10, 0); glColor3f(0.2f, 0.2f, 1.f); glVertex3f(0, 0, 0); glVertex3f(0, 0, 10); glEnd(); pangolin::FinishFrame(); } }参考高翔《视觉SLAM十四讲》P69附近