change detection_ndt_matching.

src/detection/detection_ndt_matching/main.cpp

@@ -70,6 +70,8 @@ GlobalRelocation gGlobalRelocation;
 
 bool gbNeedReloc = false;
 
+bool gbEnableReloc = true; //if have rtk position, is false;
+
 /**
  * @brief readtrace read trace
  * @param pFile
@@ -152,8 +154,9 @@ int readndtorigin(QFile * pFile,iv::gpspos * pnori)
  * @brief LoadLastPos Load last position
  * @param strlastposfilepath
  */
-static void LoadLastPos(const char * strlastposfilepath)
+static int LoadLastPos(const char * strlastposfilepath)
 {
+    int nres = -1;
     QFile * xFile = new QFile();
     xFile->setFileName(strlastposfilepath);
     if(xFile->open(QIODevice::ReadWrite))
@@ -163,6 +166,10 @@ static void LoadLastPos(const char * strlastposfilepath)
         {
             givlog->warn("load last pos fail.");
         }
+        else
+        {
+            nres = 1;
+        }
         if(gbSaveLastPos)gpFileLastPos = xFile;
         else
         {
@@ -170,6 +177,8 @@ static void LoadLastPos(const char * strlastposfilepath)
             xFile->close();
         }
     }
+
+    return nres;
 }
 
 /**
@@ -182,7 +191,7 @@ static void LoadTrace()
 
     std::string strlastposfile = strpath;
     strlastposfile = strlastposfile + "lastpos.txt";
-    LoadLastPos(strlastposfile.data());
+    int nload = LoadLastPos(strlastposfile.data());
 
     QString strpathdir = strpath.data();
     QDir dir(strpath.data());
@@ -235,6 +244,19 @@ static void LoadTrace()
             qDebug(" file not ok.");
         }
     }
+
+    if(nload != 1)
+    {
+        std::cout<<" no lastpos.txt file."<<std::endl;
+        if(gvector_trace.size() == 1)
+        {
+            std::cout<<" only one .pcd. so use .ori pos as start pos."<<std::endl;
+            glastndtgpspos = gvector_trace[0].mndtorigin;
+            std::cout<<" use ori pos as lastpos."<<std::endl;
+        }
+
+    }
+
     return;
 }
 
@@ -302,8 +324,9 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
                     point_cloud->size());
 
 
-    if(gbNeedReloc)
+    if(gbNeedReloc && gbEnableReloc)
     {
+        static int nRelocFail = 0;
         iv::Reloc xreloc = gGlobalRelocation.pointreloc(point_cloud);
         restartndtfailcount();
         if(xreloc.trans_prob>=2.0)
@@ -319,6 +342,16 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
             posereloc.roll = xreloc.roll_calc;
             setrelocpose(posereloc);
         }
+        else
+        {
+            std::cout<<" reloc fail."<<std::endl;
+            nRelocFail++;
+        }
+        if(nRelocFail >= 3)
+        {
+            std::cout<<" more than 3 reloc fail. disable reloc."<<std::endl;
+            gbEnableReloc = false;
+        }
         gbNeedReloc = false;
     }
  //
@@ -450,6 +483,7 @@ void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int
     if(xgpsimu.rtk_state() == 6)
     {
         nFixCount++;
+        gbEnableReloc = false;
     }
     else
     {