change view_ndtmatching. complete,not test.

src/tool/view_ndtmatching/mainwindow.cpp

@@ -6,6 +6,8 @@
 
 #include "showxodrinvtk.h"
 
+#include "gnss_coordinate_convert.h"
+
 #include <QMessageBox>
 #include <QFile>
 
@@ -19,6 +21,10 @@ static int gnViewMode = 0;
 
 static double gfViewAngle = 30.0;
 
+static double mflon0;
+static double mflat0;
+static bool mbLoadXODR = false;
+
 #include <cmath>
 
 struct Quaternion {
@@ -95,6 +101,10 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
 
    ShowXODRINVTK(viewer,flon0,flat0,bxodr,-1.8);
 
+   mbLoadXODR = bxodr;
+   mflon0 = flon0;
+   mflat0 = flat0;
+
    viewer.resetCamera();
 
 //   pcl::ModelCoefficients line_coeff;
@@ -229,11 +239,7 @@ void MainWindow::threadpcdview()
      char strpcpath[256];
      snprintf(strpcpath,256,"%s/map/map.pcd",getenv("HOME"));
 
-     pcl::io::loadPCDFile<pcl::PointXYZI>(strpcpath,*point_cloud);
 
- //    pcl::io::loadOBJFile("/home/yuchuli/car.obj",*point_cloud);
-//     pcl::io::loadPLYFile("/home/yuchuli/car.ply",*point_cloud);
-     mpviewer->showCloud(point_cloud);
 
 
 
@@ -246,6 +252,9 @@ void MainWindow::threadpcdview()
 
      mpviewer->runOnVisualizationThread (viewerPsycho);
 
+     pcl::io::loadPCDFile<pcl::PointXYZI>(strpcpath,*point_cloud);
+     mpviewer->showCloud(point_cloud);
+
 
      while((!mpviewer->wasStopped()) && mbRun)
      {
@@ -388,10 +397,26 @@ void MainWindow::onndtposupdate()
         xndtpos.CopyFrom(mndtpos);
         mbndtposupdate = false;
         mmutexndtpos.unlock();
-        gCurPose.x = xndtpos.pose_x();
-        gCurPose.y = xndtpos.pose_y();
+
+        double rawx = xndtpos.pose_x();
+        double rawy = xndtpos.pose_y();
+        double x,y,yaw;
+        x = rawx;
+        y = rawy;
+        yaw = xndtpos.pose_yaw();
+
+
+        if(mbLoadXODR)
+        {
+            x = rawx * cos(mfRelBaseYaw) - rawy* sin(mfRelBaseYaw) + mfRelBaseX;
+            y = rawx * sin(mfRelBaseYaw) + rawy* cos(mfRelBaseYaw) + mfRelBaseY;
+            yaw = yaw + mfRelBaseYaw;
+        }
+
+        gCurPose.x = x;
+        gCurPose.y = y;
         gCurPose.z = xndtpos.pose_z();
-        gCurPose.yaw = xndtpos.pose_yaw();
+        gCurPose.yaw = yaw;
         gCurPose.pitch = xndtpos.pose_pitch();
         gCurPose.roll = xndtpos.pose_roll();
         ui->lineEdit_prob->setText(QString::number(xndtpos.trans_probability(),'f',3));
@@ -465,9 +490,47 @@ bool MainWindow::ReadNDTOrigin(std::string strpcdpath,double & flon, double & fl
         fheading = xstr[3].toDouble();
 //        pnori->pitch = xstr[4].toDouble();
 //        pnori->roll = xstr[5].toDouble();
+
+        if(mbLoadXODR == false)
+        {
+            mfRelBaseX = 0;
+            mfRelBaseY = 0;
+            mfRelBaseYaw = 0;
+        }
+        else
+        {
+            double fx0,fy0;
+            double fx,fy;
+            GaussProjCal(mflon0,mflat0,&fx0,&fy0);
+            GaussProjCal(flon,flat,&fx,&fy);
+            mfRelBaseX = fx - fx0;
+            mfRelBaseY = fy - fy0;
+            mfRelBaseYaw = (90 - fheading) *M_PI/180.0;
+            if(mfRelBaseYaw<0)mfRelBaseYaw = mfRelBaseYaw + 2.0*M_PI;
+            if(mfRelBaseYaw>=(2.0*M_PI))mfRelBaseYaw = mfRelBaseYaw - 2.0*M_PI;
+        }
+
         return true;
     }
 
     return false;
 }
 
+
+void MainWindow::ChangePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    int nSize = point_cloud->size();
+    int i;
+    for(i=0;i<nSize;i++)
+    {
+        double rawx = point_cloud->points.at(i).x;
+        double rawy = point_cloud->points.at(i).y;
+        double x,y;
+        x = rawx * cos(mfRelBaseYaw) - rawy* sin(mfRelBaseYaw) + mfRelBaseX;
+        y = rawx * sin(mfRelBaseYaw) + rawy* cos(mfRelBaseYaw) + mfRelBaseY;
+        point_cloud->points.at(i).x = x;
+        point_cloud->points.at(i).x = y;
+
+    }
+}
+

src/tool/view_ndtmatching/mainwindow.h

@@ -67,6 +67,8 @@ private:
     void UpdateNDTPos(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
     void UpdatePCDMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
+    void ChangePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
+
 private:
     Ui::MainWindow *ui;
 
@@ -95,6 +97,12 @@ private:
     std::mutex mmutexpcdmap;
     bool mbpcdmapupdate =false;
 
+    double mfRelBaseX;
+    double mfRelBaseY;
+    double mfRelBaseYaw;
+
+
+
 
 
 };

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -20,7 +20,8 @@ SOURCES += main.cpp \
     mainwindow.cpp \
     ../../include/msgtype/ndtpos.pb.cc \
     ../../include/msgtype/relocate.pb.cc \
-    ../../include/msgtype/pcdmap.pb.cc
+    ../../include/msgtype/pcdmap.pb.cc \
+    ../../common/common/math/gnss_coordinate_convert.cpp
 
 QMAKE_LFLAGS += -no-pie
 
@@ -59,6 +60,8 @@ LIBS += -lshowxodrinvtk
 #INCLUDEPATH += $$PWD/../../../include/
 #LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault
 
+INCLUDEPATH += $$PWD/../../common/common/math/
+
 LIBS += -lboost_system
 LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
         -lvtkFiltersSources-6.3
@@ -73,7 +76,8 @@ HEADERS += \
     mainwindow.h \
     ../../include/msgtype/ndtpos.pb.h \
     ../../include/msgtype/relocate.pb.h \
-    ../../include/msgtype/pcdmap.pb.h
+    ../../include/msgtype/pcdmap.pb.h \
+    ../../common/common/math/gnss_coordinate_convert.h
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )