企业官网建设流程全解析

这个程序可以显示相机的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::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix()); pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector()); pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector()); pangolin::Var<QuaternionDraw> 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(); Matrix<double, 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附近