The function of the program: Use pcl to display the pose of the camera
and give the corresponding: R, t, euler, quaternion
. For the OpenGL part of the program , please refer to my previous article https://blog.csdn.net/joun772/article /details/109246680
Note: To install pangolin, you also need to install the visualization tool pcl
#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();//matrix 3x3
};
//运算符<<重载 对应旋转矩阵
ostream &operator<<(ostream &out, const RotationMatrix &r) {
out.setf(ios::fixed);
Matrix3d matrix = r.matrix;
out << '=';
//将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 在pcl中建立 R T euler q 列表
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
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);//取出m中的旋转矩阵
rotation_matrix = R;
TranslationVector t;
t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));//取出m的平移向量
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();//画立方体
// 画原始坐标轴
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();
}
}
The final run result
