|
|
@@ -0,0 +1,182 @@
|
|
|
+#include "mainwindow.h"
|
|
|
+#include "ui_mainwindow.h"
|
|
|
+
|
|
|
+#include <pcl/io/ply_io.h>
|
|
|
+#include <pcl/io/obj_io.h>
|
|
|
+
|
|
|
+
|
|
|
+static pose gCurPose;
|
|
|
+
|
|
|
+#include <cmath>
|
|
|
+
|
|
|
+struct Quaternion {
|
|
|
+ double w, x, y, z;
|
|
|
+};
|
|
|
+
|
|
|
+struct EulerAngles {
|
|
|
+ double roll, pitch, yaw;
|
|
|
+};
|
|
|
+
|
|
|
+EulerAngles ToEulerAngles(Quaternion q) {
|
|
|
+ EulerAngles angles;
|
|
|
+
|
|
|
+ // roll (x-axis rotation)
|
|
|
+ double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
|
|
|
+ double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
|
|
|
+ angles.roll = std::atan2(sinr_cosp, cosr_cosp);
|
|
|
+
|
|
|
+ // pitch (y-axis rotation)
|
|
|
+ double sinp = 2 * (q.w * q.y - q.z * q.x);
|
|
|
+ if (std::abs(sinp) >= 1)
|
|
|
+ angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
|
|
|
+ else
|
|
|
+ angles.pitch = std::asin(sinp);
|
|
|
+
|
|
|
+ // yaw (z-axis rotation)
|
|
|
+ double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
|
|
|
+ double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
|
|
|
+ angles.yaw = std::atan2(siny_cosp, cosy_cosp);
|
|
|
+
|
|
|
+ return angles;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
|
|
|
+{
|
|
|
+ // Abbreviations for the various angular functions
|
|
|
+ double cy = cos(yaw * 0.5);
|
|
|
+ double sy = sin(yaw * 0.5);
|
|
|
+ double cp = cos(pitch * 0.5);
|
|
|
+ double sp = sin(pitch * 0.5);
|
|
|
+ double cr = cos(roll * 0.5);
|
|
|
+ double sr = sin(roll * 0.5);
|
|
|
+
|
|
|
+ Quaternion q;
|
|
|
+ q.w = cy * cp * cr + sy * sp * sr;
|
|
|
+ q.x = cy * cp * sr - sy * sp * cr;
|
|
|
+ q.y = sy * cp * sr + cy * sp * cr;
|
|
|
+ q.z = sy * cp * cr - cy * sp * sr;
|
|
|
+
|
|
|
+ return q;
|
|
|
+}
|
|
|
+
|
|
|
+double mpos_x = 0;
|
|
|
+
|
|
|
+void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
|
|
|
+{
|
|
|
+ //设置背景颜色
|
|
|
+ viewer.setBackgroundColor(0.0,0.0,0.0);
|
|
|
+ viewer.resetCamera();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
|
|
|
+{
|
|
|
+ std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" psycho."<<std::endl;
|
|
|
+
|
|
|
+ viewer.removeShape("car");
|
|
|
+
|
|
|
+ Eigen::Vector3f trans;
|
|
|
+
|
|
|
+ Quaternion q = ToQuaternion(gCurPose.yaw,gCurPose.pitch,gCurPose.roll);
|
|
|
+ Eigen::Quaternionf quat(q.w,q.x,q.y,q.z);
|
|
|
+ trans(0) = gCurPose.x;trans(1) = gCurPose.y;trans(2) = gCurPose.z;
|
|
|
+
|
|
|
+ // viewer.addCube(mpos_x+20 -0.9,mpos_x + 20+0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
|
|
|
+ viewer.addCube(trans,quat,6.0,3.0,1.0,"car");
|
|
|
+
|
|
|
+ double yaw_calc = M_PI/2.0 - gCurPose.yaw;
|
|
|
+
|
|
|
+ viewer.setCameraPosition(gCurPose.x,gCurPose.y,100,gCurPose.x,gCurPose.y,gCurPose.z,sin(yaw_calc),cos(yaw_calc),0);
|
|
|
+
|
|
|
+ std::stringstream ss;
|
|
|
+ // viewer.setCameraPosition(mpos_x,0,30,mpos_x + 20,0,0,0,0,0);
|
|
|
+ // ss << "Once per viewer loop: " << count++;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+MainWindow::MainWindow(QWidget *parent) :
|
|
|
+ QMainWindow(parent),
|
|
|
+ ui(new Ui::MainWindow)
|
|
|
+{
|
|
|
+ ui->setupUi(this);
|
|
|
+
|
|
|
+ mCurPose.x = 0; mCurPose.y = 0; mCurPose.z = 0;
|
|
|
+ mCurPose.yaw = 0; mCurPose.pitch = 0; mCurPose.roll = 0;
|
|
|
+
|
|
|
+ gCurPose = mCurPose;
|
|
|
+\
|
|
|
+ mpthreadpcd = new std::thread(&MainWindow::threadpcdview,this);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+MainWindow::~MainWindow()
|
|
|
+{
|
|
|
+ mbRun = false;
|
|
|
+ mpthreadpcd->join();
|
|
|
+ delete ui;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void MainWindow::threadpcdview()
|
|
|
+{
|
|
|
+ mpviewer = new pcl::visualization::CloudViewer("Cloud View");
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
|
|
|
+ new pcl::PointCloud<pcl::PointXYZI>());
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud2(
|
|
|
+ new pcl::PointCloud<pcl::PointXYZI>());
|
|
|
+
|
|
|
+ pcl::io::loadPCDFile<pcl::PointXYZI>("/home/yuchuli/map/map.pcd",*point_cloud);
|
|
|
+
|
|
|
+ // pcl::io::loadOBJFile("/home/yuchuli/car.obj",*point_cloud);
|
|
|
+// pcl::io::loadPLYFile("/home/yuchuli/car.ply",*point_cloud);
|
|
|
+ mpviewer->showCloud(point_cloud);
|
|
|
+
|
|
|
+
|
|
|
+ //This will only get called once
|
|
|
+ mpviewer->runOnVisualizationThreadOnce (viewerOneOff);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ mpviewer->runOnVisualizationThread (viewerPsycho);
|
|
|
+
|
|
|
+
|
|
|
+ while((!mpviewer->wasStopped()) && mbRun)
|
|
|
+ {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ delete mpviewer;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void MainWindow::on_pushButton_Test_clicked()
|
|
|
+{
|
|
|
+ double x = ui->lineEdit_x->text().toDouble();
|
|
|
+ double y = ui->lineEdit_y->text().toDouble();
|
|
|
+ double z = ui->lineEdit_z->text().toDouble();
|
|
|
+ double yaw = ui->lineEdit_yaw->text().toDouble();
|
|
|
+ double pitch = ui->lineEdit_pitch->text().toDouble();
|
|
|
+ double roll = ui->lineEdit_roll->text().toDouble();
|
|
|
+ mCurPose.x = x;
|
|
|
+ mCurPose.y = y;
|
|
|
+ mCurPose.z = z;
|
|
|
+ mCurPose.yaw = yaw;
|
|
|
+ mCurPose.pitch = pitch;
|
|
|
+ mCurPose.roll = roll;
|
|
|
+
|
|
|
+ gCurPose = mCurPose;
|
|
|
+}
|
