change detection_ndt_matching, fix some problem. change map_collectfromveh, fix updategps.

src/detection/detection_ndt_matching/detection_ndt_matching.pro

@@ -7,7 +7,7 @@ QMAKE_CXXFLAGS += -std=gnu++17
 
 QMAKE_LFLAGS += -no-pie
 
-#QMAKE_CXXFLAGS +=  -g
+QMAKE_CXXFLAGS +=  -g
 
 QMAKE_CXXFLAGS += -O3
 

src/detection/detection_ndt_matching/globalrelocation.cpp

@@ -6,6 +6,7 @@
 
 GlobalRelocation::GlobalRelocation()
 {
+//    std::cout<<" run hear."<<std::endl;
     if(mnThreadNum > MAX_NDT)mnThreadNum = MAX_NDT;
     int i;
     for(i=0;i<MAX_NDT;i++)mpthread[i] = NULL;
@@ -408,69 +409,70 @@ void GlobalRelocation::setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap)
         mbRelocAvail = true;
         mndt[i].setInputTarget(xmap);
         mthreadcomplete[i] = true;
+    }
+
 
-        int i;
-        int npointsize = mvectorndtmap[0].mvector_trace.size();
-        if(npointsize == 0)
+    int npointsize = mvectorndtmap[0].mvector_trace.size();
+    if(npointsize == 0)
+    {
+        return;
+    }
+    mvectorseedpoint.clear();
+    std::cout<<" load trace seed."<<std::endl;
+    iv::ndttracepoint ntp = mvectorndtmap[0].mvector_trace[0];
+    mvectorseedpoint.push_back(ntp);
+
+    for(i=1;i<npointsize;i++)
+    {
+        iv::ndttracepoint ntpnow = mvectorndtmap[0].mvector_trace[i];
+        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
+        double fdis = sqrt(pow(ntpnow.x - ntplast.x,2) + pow(ntpnow.y - ntplast.y,2));
+        double fyawdiff = ntpnow.yaw - ntplast.yaw;
+        while(fyawdiff<(-M_PI))fyawdiff = fyawdiff + 2.0*M_PI;
+        while(fyawdiff >(M_PI))fyawdiff = fyawdiff - 2.0*M_PI;
+        if(fdis>=mfSeedDisThresh)
         {
-            return;
+            mvectorseedpoint.push_back(ntpnow);
         }
-        mvectorseedpoint.clear();
-        std::cout<<" load trace seed."<<std::endl;
-        iv::ndttracepoint ntp = mvectorndtmap[0].mvector_trace[0];
-        mvectorseedpoint.push_back(ntp);
-
-        for(i=1;i<npointsize;i++)
+        else
         {
-            iv::ndttracepoint ntpnow = mvectorndtmap[0].mvector_trace[i];
-            iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
-            double fdis = sqrt(pow(ntpnow.x - ntplast.x,2) + pow(ntpnow.y - ntplast.y,2));
-            double fyawdiff = ntpnow.yaw - ntplast.yaw;
-            while(fyawdiff<(-M_PI))fyawdiff = fyawdiff + 2.0*M_PI;
-            while(fyawdiff >(M_PI))fyawdiff = fyawdiff - 2.0*M_PI;
-            if(fdis>=mfSeedDisThresh)
+            if(fabs(fyawdiff)>0.1)
             {
                 mvectorseedpoint.push_back(ntpnow);
             }
             else
             {
-                if(fabs(fyawdiff)>0.1)
+                if(i== (npointsize -1))
                 {
                     mvectorseedpoint.push_back(ntpnow);
                 }
-                else
-                {
-                    if(i== (npointsize -1))
-                    {
-                       mvectorseedpoint.push_back(ntpnow);
-                    }
-                }
             }
         }
+    }
 
-        if(mvectorseedpoint.size()>0)
+    if(mvectorseedpoint.size()>0)
+    {
+        const int nfrontadd = 3;
+        const int nrearadd  = 3;
+        iv::ndttracepoint ntpfirst = mvectorseedpoint[0];
+        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
+        for(i=0;i<nfrontadd;i++)
         {
-            const int nfrontadd = 3;
-            const int nrearadd  = 3;
-            iv::ndttracepoint ntpfirst = mvectorseedpoint[0];
-            iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
-            for(i=0;i<nfrontadd;i++)
-            {
-                iv::ndttracepoint ntpnow = ntpfirst;
-                double foff = (i+1)*mfSeedDisThresh;
-                ntpnow.x = ntpfirst.x + foff*cos(ntpfirst.yaw + M_PI);
-                ntpnow.y = ntpfirst.y + foff*sin(ntpfirst.yaw + M_PI);
-                mvectorseedpoint.insert(mvectorseedpoint.begin(),ntpnow);
-            }
-            for(i=0;i<nrearadd;i++)
-            {
-                iv::ndttracepoint ntpnow = ntplast;
-                double foff = (i+1)*mfSeedDisThresh;
-                ntpnow.x = ntpfirst.x + foff*cos(ntpfirst.yaw);
-                ntpnow.y = ntpfirst.y + foff*sin(ntpfirst.yaw );
-                mvectorseedpoint.push_back(ntpnow);
-            }
+            iv::ndttracepoint ntpnow = ntpfirst;
+            double foff = (i+1)*mfSeedDisThresh;
+            ntpnow.x = ntpfirst.x + foff*cos(ntpfirst.yaw + M_PI);
+            ntpnow.y = ntpfirst.y + foff*sin(ntpfirst.yaw + M_PI);
+            mvectorseedpoint.insert(mvectorseedpoint.begin(),ntpnow);
+        }
+        for(i=0;i<nrearadd;i++)
+        {
+            iv::ndttracepoint ntpnow = ntplast;
+            double foff = (i+1)*mfSeedDisThresh;
+            ntpnow.x = ntplast.x + foff*cos(ntpfirst.yaw);
+            ntpnow.y = ntplast.y + foff*sin(ntpfirst.yaw );
+            mvectorseedpoint.push_back(ntpnow);
         }
+    }
 
 //        for(i=0;i<mvectorseedpoint.size();i++)
 //        {
@@ -478,5 +480,5 @@ void GlobalRelocation::setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap)
 //            mvectorseedpoint[i].y = mvectorseedpoint[i].y + 10.0;
 //        }
 
-    }
+
 }

src/detection/detection_ndt_matching/main.cpp

@@ -574,6 +574,7 @@ void threadrelocation()
 int main(int argc, char *argv[])
 {
     showversion("detection_ndt_matching");
+    RegisterIVBackTrace();
     QCoreApplication a(argc, argv);
 
     QString strpath = QCoreApplication::applicationDirPath();
@@ -652,7 +653,7 @@ int main(int argc, char *argv[])
     ndt_match_Init_nomap();
 
     iv::ivexit::RegIVExitCall(exitfunc);
-    RegisterIVBackTrace();
+
 
     gpthread = new std::thread(statethread);
 //    gpb = iv::modulecomm::RegisterRecv("ndt_mappath",ListenMapUpdate);

src/tool/map_collectfromveh/commif.cpp

@@ -17,7 +17,7 @@ void UpdateGPS(const char *strdata, const unsigned int nSize, const unsigned int
 
     int64_t nNow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
 
-    if(strncmp(strdata,"fusion_gpsimu",256)!= 0)
+    if(strncmp(strmemname,"fusion_gpsimu",256)!= 0)
     {
         if(abs(nNow - gnLastFusionGPSUpdate)<1000)
         {