change view_ndtmatching. add 3d point plan.

src/tool/pointcloudviewer/main.cpp

@@ -159,7 +159,7 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
 void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
 {
     std::cout<<" PCL_VERSION : "<<PCL_VERSION<<std::endl;
-    if(PCL_VERSION > 101200) viewer.getRenderWindow()->GlobalWarningDisplayOff(); // Add This Line
+ //   if(PCL_VERSION > 101200) viewer.getRenderWindow()->GlobalWarningDisplayOff(); // Add This Line
 
     std::cout<<" run hear."<<std::endl;
     //设置背景颜色

src/tool/pointcloudviewer/pointcloudviewer.pro

@@ -71,9 +71,9 @@ LIBS += -lboost_system
 #LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
 #       -lvtkFiltersSources-9.1
 
-#LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
-#        -lvtkFiltersSources-7.1
+LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
+        -lvtkFiltersSources-7.1
 
 
-LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
-        -lvtkFiltersSources-6.3  -lvtkRenderingFreeType-6.3
+#LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
+#        -lvtkFiltersSources-6.3  -lvtkRenderingFreeType-6.3

src/tool/view_ndtmatching/mainwindow.cpp

@@ -118,8 +118,15 @@ void ShowXODR(pcl::visualization::PCLVisualizer& viewer)
     viewer.addLine(mass_center,kinectZ2,1.0f,1.0f,1.0f,"xodr2");
 }
 
+pcl::visualization::PCLVisualizer *gpviewer;
+
+#include <vtkRenderer.h>
+#include <vtkRenderWindow.h>
+
 void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
 {
+    gpviewer = &viewer;
+//    viewer.registerMouseCallback(mouseEventOccurred,(void *)&viewer);
     //设置背景颜色
    viewer.setBackgroundColor(0.0,0.0,0.0);
 
@@ -138,6 +145,8 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
    viewer.resetCamera();
 
 
+
+
    if (!pcl::io::loadOBJFile("./car.obj", mesh))
    {
        std::cout << "error"<<std::endl;
@@ -198,6 +207,8 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
  //  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)
@@ -401,6 +412,7 @@ MainWindow::MainWindow(QWidget *parent) :
     mpthreadpcd = new std::thread(&MainWindow::threadpcdview,this);
 
 
+    mpadst = iv::modulecomm::RegisterSend("xodrreq",1000,1);
     ModuleFun fun1 =std::bind(&MainWindow::UpdateNDTPos,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpandtpos = iv::modulecomm::RegisterRecvPlus("ndtpos",fun1);
 
@@ -419,6 +431,7 @@ MainWindow::MainWindow(QWidget *parent) :
     mpacnndetect  = iv::modulecomm::RegisterRecvPlus("lidar_track",funcnndetect);
 
     connect(this,SIGNAL(ndtposupdate()),this,SLOT(onndtposupdate()));
+    connect(this,SIGNAL(updatedst(double,double)),this,SLOT(onUpdateDst(double,double)));
 
     setWindowTitle("NDT Matching View & Relocation Enable");
 
@@ -438,6 +451,8 @@ MainWindow::~MainWindow()
 }
 
 
+
+
 void MainWindow::threadpcdview()
 {
      mpviewer = new pcl::visualization::CloudViewer("Cloud View");
@@ -458,7 +473,7 @@ void MainWindow::threadpcdview()
 
 
      //This will only get called once
- //    mpviewer->runOnVisualizationThreadOnce (viewerOneOff);
+     mpviewer->runOnVisualizationThreadOnce (viewerOneOff);
 
 
      std::this_thread::sleep_for(std::chrono::milliseconds(1000));
@@ -466,6 +481,9 @@ void MainWindow::threadpcdview()
 
      mpviewer->runOnVisualizationThread (viewerPsycho);
 
+//     mpviewer->registerMouseCallback(mouseEventOccurred,(void *)mpviewer);
+     mpviewer->registerMouseCallback(&MainWindow::mouseEventOccurred,*this,(void *)mpviewer);
+
      pcl::io::loadPCDFile<pcl::PointXYZI>(strpcpath,*point_cloud);
 
      double flon,flat,fheading;
@@ -837,3 +855,60 @@ void MainWindow::ChangePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_c
     }
 }
 
+
+void MainWindow::on_pushButton_Plan_clicked()
+{
+    QString strlon = ui->lineEdit_Lon->text();
+    QString strlat = ui->lineEdit_Lat->text();
+    if((strlon.length()<1) || (strlat.length()<1))
+    {
+        QMessageBox::warning(this,tr("Warning"),tr("Please Set Lon lat."),QMessageBox::YesAll);
+        return;
+    }
+
+    double lon,lat;
+    lon = ui->lineEdit_Lon->text().toDouble();
+    lat = ui->lineEdit_Lat->text().toDouble();
+
+    xodrobj xo;
+    xo.flon = lon;
+    xo.flat = lat;
+    xo.lane = 1;//mpLE_LaneSel->text().toInt();
+
+    iv::modulecomm::ModuleSendMsg(mpadst,(char *)&xo,sizeof(xodrobj));
+}
+
+void MainWindow::mouseEventOccurred (const pcl::visualization::MouseEvent &event,
+                     void* viewer_void)
+{
+    if (event.getButton () == pcl::visualization::MouseEvent::LeftButton && event.getType () == pcl::visualization::MouseEvent::MouseDblClick)
+    {
+        vtkRenderer* renderer = gpviewer->getRenderWindow()->GetRenderers()->GetFirstRenderer() ;
+        renderer->SetDisplayPoint(event.getX (), event.getY (), 0);
+        renderer->DisplayToWorld();
+        double w_x = (renderer->GetWorldPoint())[0];
+        double w_y = (renderer->GetWorldPoint())[1];
+        double w_z  = (renderer->GetWorldPoint())[2];
+
+        std::cout<<"w_x: "<<w_x<<" w_y:"<<w_y<<" w_z:"<<w_z<<std::endl;
+
+        emit updatedst(w_x,w_y);
+
+
+
+
+    }
+}
+
+void MainWindow::onUpdateDst(double w_x, double w_y)
+{
+    double x0,y0;
+    GaussProjCal(mflon0,mflat0,&x0,&y0);
+    double x, y;
+    x = x0 + w_x;
+    y = y0 + w_y;
+    double lon,lat;
+    GaussProjInvCal(x,y,&lon,&lat);
+    ui->lineEdit_Lon->setText(QString::number(lon,'f',7));
+    ui->lineEdit_Lat->setText(QString::number(lat,'f',7));
+}

src/tool/view_ndtmatching/mainwindow.h

@@ -32,6 +32,17 @@ struct pose
   double vy;
 };
 
+class xodrobj
+{
+public:
+    double flatsrc;
+    double flonsrc;
+    double fhgdsrc;
+    double flat;
+    double flon;
+    int lane;
+};
+
 namespace Ui {
 class MainWindow;
 }
@@ -46,6 +57,7 @@ public:
 
 signals:
     void ndtposupdate();
+    void updatedst(double x0,double y0);
 private slots:
     void on_pushButton_Test_clicked();
 
@@ -59,6 +71,10 @@ private slots:
 
     void onndtposupdate();
 
+    void on_pushButton_Plan_clicked();
+
+    void onUpdateDst(double x0,double y0);
+
 private:
     void threadpcdview();
 
@@ -90,6 +106,7 @@ private:
     void * mpagpsimu;
     void *mpafusiongpsimu;
     void * mpacnndetect;
+    void * mpadst;
 
     pose mCurPose;
 
@@ -113,8 +130,9 @@ private:
     int64_t mnLastFusionGPSIMUUpdate = 0;
     int64_t mnLastNDTUpdate = 0;
 
-
-
+private:
+    void mouseEventOccurred (const pcl::visualization::MouseEvent &event,
+                         void* viewer_void);
 
 
 

src/tool/view_ndtmatching/mainwindow.ui

@@ -266,6 +266,65 @@
      <string>prob</string>
     </property>
    </widget>
+   <widget class="QLineEdit" name="lineEdit_Lon">
+    <property name="geometry">
+     <rect>
+      <x>120</x>
+      <y>460</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Lat">
+    <property name="geometry">
+     <rect>
+      <x>370</x>
+      <y>460</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_9">
+    <property name="geometry">
+     <rect>
+      <x>290</x>
+      <y>460</y>
+      <width>61</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Lat:</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_10">
+    <property name="geometry">
+     <rect>
+      <x>50</x>
+      <y>460</y>
+      <width>61</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Lon:</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="pushButton_Plan">
+    <property name="geometry">
+     <rect>
+      <x>530</x>
+      <y>453</y>
+      <width>131</width>
+      <height>41</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Plan</string>
+    </property>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">
@@ -273,7 +332,7 @@
      <x>0</x>
      <y>0</y>
      <width>702</width>
-     <height>22</height>
+     <height>28</height>
     </rect>
    </property>
   </widget>

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -73,11 +73,11 @@ LIBS += -lboost_system
 #LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
 #        -lvtkFiltersSources-6.3
 
-LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
-        -lvtkFiltersSources-9.1
+#LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+#        -lvtkFiltersSources-9.1
 
-#LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
-#        -lvtkFiltersSources-7.1
+LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
+        -lvtkFiltersSources-7.1
 
 FORMS += \
     mainwindow.ui

src/tool/view_showxodrinvtk/showxodrinvtk.pro

@@ -77,11 +77,11 @@ LIBS += -lboost_system
 #LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
 #        -lvtkFiltersSources-6.3
 
-LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
-        -lvtkFiltersSources-9.1
+#LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+#        -lvtkFiltersSources-9.1
 
-#LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
-#        -lvtkFiltersSources-7.1
+LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
+        -lvtkFiltersSources-7.1
 
 HEADERS += \
     mconf.h \