change detection_ndt_matching.

src/detection/detection_ndt_matching/globalrelocation.cpp

@@ -204,18 +204,22 @@ void GlobalRelocation::RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::Poi
 }
 
 
-void GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
+iv::Reloc GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 {
+    iv::Reloc xreloczero;
+    xreloczero.trans_prob = 0.0;
+
     if(mvectorndtmap.size() > 1)
     {
         std::cout<<" only one map, can use relocation."<<std::endl;
-        return;
+
+        return xreloczero;
     }
 
     if(mvectorndtmap.size() == 0)
     {
         std::cout<<" no map."<<std::endl;
-        return;
+        return xreloczero;
     }
 
     mvectorReloc.clear();
@@ -245,7 +249,8 @@ void GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
     if(fMaxProb > 2.0)
     {
-        return;
+
+        return mvectorReloc[indexmax];
     }
 
     std::cout<<" use diff yaw"<<std::endl;
@@ -285,7 +290,7 @@ void GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
     if(fMaxProb > 2.0)
     {
-        return;
+        return mvectorReloc[indexmax];
     }
 
     std::cout<<" use dis yaw"<<std::endl;
@@ -344,9 +349,19 @@ void GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     if(fMaxProb > 2.0)
     {
         std::cout<<" find pos."<<std::endl;
-        return;
+        return mvectorReloc[indexmax];
     }
 
+
+    if(mvectorReloc.size() > 0)
+    {
+        return mvectorReloc[indexmax];
+    }
+
+
+    return xreloczero;
+
+
 }
 
 void GlobalRelocation::setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap)

src/detection/detection_ndt_matching/globalrelocation.h

@@ -34,7 +34,7 @@ class GlobalRelocation
 public:
     GlobalRelocation();
 
-    void pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+    iv::Reloc pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
 
     void setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap);
 

src/detection/detection_ndt_matching/main.cpp

@@ -68,6 +68,8 @@ QFile * gpFileLastPos = 0;//Save Last Positin
 
 GlobalRelocation gGlobalRelocation;
 
+bool gbNeedReloc = false;
+
 /**
  * @brief readtrace read trace
  * @param pFile
@@ -300,7 +302,26 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
                     point_cloud->size());
 
 
- //   gGlobalRelocation.pointreloc(point_cloud);
+    if(gbNeedReloc)
+    {
+        iv::Reloc xreloc = gGlobalRelocation.pointreloc(point_cloud);
+        restartndtfailcount();
+        if(xreloc.trans_prob>=2.0)
+        {
+            pose posereloc;
+            posereloc.vx = 0;
+            posereloc.vy = 0;
+            posereloc.x = xreloc.x_calc;
+            posereloc.y = xreloc.y_calc;
+            posereloc.z = xreloc.z_calc;
+            posereloc.yaw = xreloc.yaw_calc;
+            posereloc.pitch = xreloc.pitch_calc;
+            posereloc.roll = xreloc.roll_calc;
+            setrelocpose(posereloc);
+        }
+        gbNeedReloc = false;
+    }
+ //
 
     point_ndt(point_cloud);
     gndttime = xTime.elapsed();
@@ -508,6 +529,14 @@ void exitfunc()
     std::cout<<"Complete exitfunc."<<std::endl;
 }
 
+void threadrelocation()
+{
+    while(gbstate)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
 int main(int argc, char *argv[])
 {
     showversion("detection_ndt_matching");

src/detection/detection_ndt_matching/ndt_matching.cpp

@@ -114,6 +114,8 @@ void * gppose;
 
 static bool g_hasmap = false;
 
+extern bool gbNeedReloc;
+
 enum class MethodType
 {
   PCL_GENERIC = 0,
@@ -250,6 +252,10 @@ static bool gbFileNDTUpdate;
 extern double garm_x ;
 extern double garm_y ;
 
+static int gndtcalcfailcount = 0;
+static pose gPoseReloc;
+static bool gbPoseReloc = false;
+
 #include <QDateTime>
 
 //cv::Mat gmatimage;
@@ -1201,6 +1207,18 @@ void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double
 }
 
 
+void restartndtfailcount()
+{
+    gndtcalcfailcount = 0;
+}
+
+void setrelocpose(pose xPose)
+{
+    gPoseReloc = xPose;
+    gbPoseReloc = true;
+}
+
+
 #ifdef TEST_CALCTIME
 int ncalc = 0;
 int gncalctotal = 0;
@@ -1215,6 +1233,8 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
     bool bNotChangev = false;
 
+
+
     if(gbgpsupdatendt == true)
     {
 
@@ -1275,6 +1295,13 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
 
     previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+
+    if(gbPoseReloc)
+    {
+        previous_pose = gPoseReloc;
+        gbPoseReloc = false;
+    }
+
  //   std::cout<<" previos head: "<<previous_pose.yaw<<std::endl;
     if(bNeedUpdateVel == true)
     {
@@ -1518,6 +1545,21 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
       }
 
 
+      if(trans_probability >= 1.0)
+      {
+          gndtcalcfailcount = 0;
+      }
+      else
+      {
+          gndtcalcfailcount++;
+      }
+
+      if(gndtcalcfailcount >= 30)
+      {
+          gbNeedReloc = true;
+      }
+
+
       std::cout<<"score: "<<fitness_score<<" trans_probability:"<<trans_probability<<std::endl;
   //    std::cout<<" scoure is "<<fitness_score<<std::endl;
 

src/detection/detection_ndt_matching/ndt_matching.h

@@ -64,6 +64,10 @@ void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);
 
 void SetRunState(bool bRun);
 
+void restartndtfailcount();
+
+void setrelocpose(pose xPose);
+
 namespace iv {
 struct ndttracepoint
 {