change view_ndtmatching.

src/tool/view_ndtmatching/main.cpp

@@ -2,9 +2,44 @@
 
 #include <QApplication>
 
+#include "xmlparam.h"
+
+
+double gfcustom_cam_x = -50;
+double gfcustom_cam_y = 0;
+double gfcustom_cam_z = 30;
+double gfcustom_view_x = 0;
+double gfcustom_view_y = 0;
+double gfcustom_view_z = 0;
+double gfcustom_yawrotate = 0;
+
+
+void LoadCameraPos(QString strpath)
+{
+    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+    gfcustom_cam_x = xparam.GetParam("custom_cam_x",-50.0);
+    gfcustom_cam_y = xparam.GetParam("custom_cam_y",0.0);
+    gfcustom_cam_z = xparam.GetParam("custom_cam_z",0.0);
+    gfcustom_view_x = xparam.GetParam("custom_view_x",0.0);
+    gfcustom_view_y = xparam.GetParam("custom_view_y",0.0);
+    gfcustom_view_z = xparam.GetParam("custom_view_z",0.0);
+    gfcustom_yawrotate = xparam.GetParam("custom_yawrotate",0.0) *M_PI/180.0;
+}
+
 int main(int argc, char *argv[])
 {
     QApplication a(argc, argv);
+
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+   if(argc < 2)
+       strpath = strpath + "/view_ndtmatching.xml";
+   else
+       strpath = argv[1];
+
+
+    LoadCameraPos(strpath);
     MainWindow w;
     w.show();
     return a.exec();

src/tool/view_ndtmatching/mainwindow.cpp

@@ -27,6 +27,14 @@ static double mflon0;
 static double mflat0;
 static bool mbLoadXODR = false;
 
+extern double gfcustom_cam_x;
+extern double gfcustom_cam_y;
+extern double gfcustom_cam_z;
+extern double gfcustom_view_x;
+extern double gfcustom_view_y;
+extern double gfcustom_view_z;
+extern double gfcustom_yawrotate;
+
 #include <cmath>
 
 struct Quaternion {
@@ -159,6 +167,18 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
             double fcam_y = gCurPose.y + gfZcos * sin(gCurPose.yaw+M_PI);
             viewer.setCameraPosition(fcam_x,fcam_y,gfZsin,gCurPose.x,gCurPose.y,gCurPose.z,sin(yaw_calc),cos(yaw_calc),0);
         }
+        if(gnViewMode == 2)
+        {
+            double fcam_x,fcam_y,fcam_z;
+            fcam_z = gfcustom_cam_z;
+            fcam_x = gCurPose.x + gfcustom_cam_x * cos(gCurPose.yaw) - gfcustom_cam_y * sin(gCurPose.yaw);
+            fcam_y = gCurPose.y + gfcustom_cam_x * sin(gCurPose.yaw) + gfcustom_cam_y * cos(gCurPose.yaw);
+            yaw_calc = yaw_calc + gfcustom_yawrotate;
+            double fview_x = gCurPose.x + gfcustom_view_x * cos(gCurPose.yaw) - gfcustom_view_y * sin(gCurPose.yaw);
+            double fview_y = gCurPose.y + gfcustom_view_x * sin(gCurPose.yaw) + gfcustom_view_y * sin(gCurPose.yaw);
+            double fview_z = gfcustom_view_z + gCurPose.z;
+            viewer.setCameraPosition(fcam_x,fcam_y,fcam_z,fview_x,fview_y,fview_z,sin(yaw_calc),cos(yaw_calc),0);
+        }
     }
 
     std::stringstream ss;
@@ -189,6 +209,7 @@ MainWindow::MainWindow(QWidget *parent) :
 
     ui->comboBox->addItem("Look Down");
     ui->comboBox->addItem("Tracking");
+    ui->comboBox->addItem("Custom");
 
     ui->checkBox->setChecked(true);
 
@@ -414,6 +435,13 @@ void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,cons
     }
 
 
+    //IF Have NDT Position,Use NDT Position.
+    int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    if(abs(nmsnow - mnLastNDTUpdate)<3000)
+    {
+        return;
+    }
+
 
     if(mbLoadXODR)
     {
@@ -441,6 +469,7 @@ void MainWindow::UpdateNDTPos(const char * strdata,const unsigned int nSize,cons
     (void)dt;
     (void)strmemname;
 
+
     iv::lidar::ndtpos xndtpos;
     if(xndtpos.ParseFromArray(strdata,nSize))
     {
@@ -449,6 +478,8 @@ void MainWindow::UpdateNDTPos(const char * strdata,const unsigned int nSize,cons
         mbndtposupdate = true;
         mmutexndtpos.unlock();
         emit ndtposupdate();
+
+        mnLastNDTUpdate = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
     }
     else
     {

src/tool/view_ndtmatching/mainwindow.h

@@ -107,6 +107,7 @@ private:
     double mfRelBaseYaw;
 
     int64_t mnLastFusionGPSIMUUpdate = 0;
+    int64_t mnLastNDTUpdate = 0;
 
 
 

src/tool/view_ndtmatching/mainwindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>686</width>
-    <height>499</height>
+    <width>702</width>
+    <height>566</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -272,8 +272,8 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>686</width>
-     <height>28</height>
+     <width>702</width>
+     <height>22</height>
     </rect>
    </property>
   </widget>

src/tool/view_ndtmatching/view_ndtmatching.xml

@@ -0,0 +1,12 @@
+<xml>	
+	<node name="view_ndtmatching.xml">
+		<param name="custom_cam_x" value="-50.0" />
+		<param name="custom_cam_y" value="0.0" />
+		<param name="custom_cam_z" value="0.0" />
+		<param name="custom_view_x" value="0.0" />
+		<param name="custom_view_y" value="0.0" />
+		<param name="custom_view_z" value="0.0" />
+		<param name="custom_yawrotate" value="0.0" />
+
+	</node>
+</xml>