change detection_ndt_matching. add front and rear extend, add thread pool join.

src/detection/detection_ndt_matching/globalrelocation.cpp

@@ -6,7 +6,9 @@
 
 GlobalRelocation::GlobalRelocation()
 {
-
+    if(mnThreadNum > MAX_NDT)mnThreadNum = MAX_NDT;
+    int i;
+    for(i=0;i<MAX_NDT;i++)mpthread[i] = NULL;
 }
 
 void GlobalRelocation::threadtest(int n,iv::Reloc & xReloc)
@@ -25,6 +27,7 @@ void GlobalRelocation::threadReloc(int nThread,cpu::NormalDistributionsTransform
     xReloc.yaw_calc = xReloc.yaw_guess;
     xReloc.pitch_calc = 0;// xReloc.pitch_guess;
     xReloc.roll_calc = 0;//xReloc.roll_guess;
+    xReloc.trans_prob = 0;
 
 
     Eigen::Matrix4f tf_btol;
@@ -150,6 +153,12 @@ void GlobalRelocation::RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::Poi
             {
                 if((mthreadcomplete[i] == true) &&(bHaveTask))
                 {
+                    if(mpthread[i] != NULL)
+                    {
+                        mpthread[i]->join();
+                        delete mpthread[i];
+                        mpthread[i] = NULL;
+                    }
                     iv::Reloc xReloc;
                     xReloc.x_guess = xtrace[j].x;
                     xReloc.y_guess = xtrace[j].y;
@@ -159,8 +168,9 @@ void GlobalRelocation::RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::Poi
                     xReloc.roll_guess = 0;
                     if(j == ntracesize)bHaveTask = false;
     //                std::thread * pthread = new std::thread(&GlobalRelocation::threadtest,this,i,xReloc);
-                    std::thread * pthread = new std::thread(&GlobalRelocation::threadReloc,this,i,&mndt[i],raw_scan,xReloc);
                     mthreadcomplete[i] = false;
+                    mpthread[i] = new std::thread(&GlobalRelocation::threadReloc,this,i,&mndt[i],raw_scan,xReloc);
+
                     j++;
                     if(j == ntracesize)bHaveTask = false;
                 }
@@ -172,7 +182,12 @@ void GlobalRelocation::RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::Poi
             {
                 if((mthreadcomplete[i] == true))
                 {
-
+                    if(mpthread[i] != NULL)
+                    {
+                        mpthread[i]->join();
+                        delete mpthread[i];
+                        mpthread[i] = NULL;
+                    }
                 }
                 else
                 {
@@ -423,6 +438,37 @@ void GlobalRelocation::setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap)
                 {
                     mvectorseedpoint.push_back(ntpnow);
                 }
+                else
+                {
+                    if(i== (npointsize -1))
+                    {
+                       mvectorseedpoint.push_back(ntpnow);
+                    }
+                }
+            }
+        }
+
+        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++)
+            {
+                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);
             }
         }
 

src/detection/detection_ndt_matching/globalrelocation.h

@@ -66,6 +66,8 @@ private:
 
     int mnThreadNum = 8;
 
+    std::thread * mpthread[MAX_NDT];
+
     const double mfSeedDisThresh = 3.0;
     const int mnCirclDivide  = 20;