change adcndtmaching, add anh method.change detection_ndt_matching,add anh method.

src/detection/detection_ndt_matching/detection_ndt_mathcing.pro → src/detection/detection_ndt_matching/detection_ndt_matching.pro

@@ -7,7 +7,9 @@ QMAKE_CXXFLAGS += -std=gnu++17
 
 QMAKE_LFLAGS += -no-pie
 
-QMAKE_CXXFLAGS +=  -g
+#QMAKE_CXXFLAGS +=  -g
+
+QMAKE_CXXFLAGS += -O3
 
 # The following define makes your compiler emit warnings if you use
 # any feature of Qt which as been marked deprecated (the exact warnings
@@ -77,10 +79,12 @@ unix:LIBS +=  -lpcl_common\
     error( "Couldn't find the common.pri file!" )
 }
 
+INCLUDEPATH += $$PWD/../../common/ndt_cpu/
+
 
 #DEFINES+= TEST_CALCTIME
 
 INCLUDEPATH += $$PWD/../../../include/
-LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_gpu  -livexit -livbacktrace
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_cpu  -livexit -livbacktrace
 
 

src/detection/detection_ndt_matching/detection_ndt_matching.xml

@@ -0,0 +1,14 @@
+<xml>	
+	<node name="detection_ndt_matching.xml">
+		<param name="lidarmsg" value="lidar_pc" />
+		<param name="gpsmsg" value="hcp2_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.0" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+		<param name="useanh" value="false" />
+	</node>
+</xml>

src/detection/detection_ndt_matching/main.cpp

@@ -21,6 +21,8 @@
 #include "xmlparam.h"
 #include "ivversion.h"
 
+extern bool gbuseanh;
+
 
 QTime gTime;    //a Time function. use calculate elapsed.
 
@@ -503,13 +505,13 @@ void exitfunc()
 
 int main(int argc, char *argv[])
 {
-    showversion("detection_ndt_matching_gpu_multi");
+    showversion("detection_ndt_matching");
     QCoreApplication a(argc, argv);
 
     QString strpath = QCoreApplication::applicationDirPath();
 
    if(argc < 2)
-       strpath = strpath + "/detection_ndt_matching_gpu_multi.xml";
+       strpath = strpath + "/detection_ndt_matching.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;
@@ -529,6 +531,8 @@ int main(int argc, char *argv[])
    gstr_arm_x = xparam.GetParam("Arm_LidarGPSBase_x","0.0");
    gstr_arm_y = xparam.GetParam("Arm_LidarGPSBase_y","0.0");
 
+   gbuseanh = xparam.GetParam("useanh",false);
+
    gyawcorrect = atof(gstr_yawcorrect.data()) * M_PI/180.0;
    gheightcorrect = atof(gstr_heightcorrect.data());
 

src/detection/detection_ndt_matching/ndt_matching.cpp

@@ -64,7 +64,7 @@
 #include <pcl_ros/point_cloud.h>
 #include <pcl_ros/transforms.h>
 
-
+#include <ndt_cpu/NormalDistributionsTransform.h>
 
 #include "ndt_matching.h"
 
@@ -140,9 +140,12 @@ static int map_loaded = 0;
 static int _use_gnss = 1;
 static int init_pos_set = 0;
 
-static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+bool gbuseanh = true;
 
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
 
+static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
+static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> glocalanh_ndt;
 
 static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
 static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
@@ -488,22 +491,41 @@ static void UseMap(int index)
     gcurmapindex = index;
 
 
+    if(gbuseanh)
+    {
+        cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> localanhraw;
 
-    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+        localanhraw.setResolution(ndt_res);
+        localanhraw.setInputTarget(map_ptr);
+        localanhraw.setMaximumIterations(max_iter);
+        localanhraw.setStepSize(step_size);
+        localanhraw.setTransformationEpsilon(trans_eps);
 
+        pthread_mutex_lock(&mutex);
+        //        ndt = glocal_ndt;
+        glocalanh_ndt = localanhraw;
+        pthread_mutex_unlock(&mutex);
 
-    localndtraw->setResolution(ndt_res);
-    localndtraw->setInputTarget(map_ptr);
-    localndtraw->setMaximumIterations(max_iter);
-    localndtraw->setStepSize(step_size);
-    localndtraw->setTransformationEpsilon(trans_eps);
+        gbNeedUpdate = true;
+    }
+    else
+    {
 
-    pthread_mutex_lock(&mutex);
-//        ndt = glocal_ndt;
-    glocalndtraw = localndtraw;
-    pthread_mutex_unlock(&mutex);
+        pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
 
-    gbNeedUpdate = true;
+        localndtraw->setResolution(ndt_res);
+        localndtraw->setInputTarget(map_ptr);
+        localndtraw->setMaximumIterations(max_iter);
+        localndtraw->setStepSize(step_size);
+        localndtraw->setTransformationEpsilon(trans_eps);
+
+        pthread_mutex_lock(&mutex);
+        //        ndt = glocal_ndt;
+        glocalndtraw = localndtraw;
+        pthread_mutex_unlock(&mutex);
+
+        gbNeedUpdate = true;
+    }
 
 
     std::cout<<"change map, index is "<<index<<std::endl;
@@ -1222,9 +1244,19 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     }
     if(gbNeedUpdate)
     {
-        pthread_mutex_lock(&mutex);
-        ndtraw = glocalndtraw;
-        pthread_mutex_unlock(&mutex);
+        if(gbuseanh)
+        {
+            pthread_mutex_lock(&mutex);
+            anh_ndt = glocalanh_ndt;
+            pthread_mutex_unlock(&mutex);
+        }
+        else
+        {
+            pthread_mutex_lock(&mutex);
+            ndtraw = glocalndtraw;
+            pthread_mutex_unlock(&mutex);
+        }
+
         gusemapindex = gcurmapindex;
         gbNeedUpdate = false;
 
@@ -1295,7 +1327,20 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
  //   ndt.setInputSource(filtered_scan_ptr);
 
 
-    ndtraw->setInputSource(filtered_scan_ptr);
+    if(gbuseanh)
+    {
+#ifdef  TEST_CALCTIME
+        std::cout<<"use anh."<<std::endl;
+#endif
+        anh_ndt.setInputSource(filtered_scan_ptr);
+    }
+    else
+    {
+#ifdef  TEST_CALCTIME
+        std::cout<<"use ndt."<<std::endl;
+#endif
+        ndtraw->setInputSource(filtered_scan_ptr);
+    }
 
     // Guess the initial gross estimation of the transformation
 //    double diff_time = (current_scan_time - previous_scan_time).toSec();
@@ -1411,7 +1456,12 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 //      std::cout<<"time is "<<xTime.elapsed()<<std::endl;
       pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
       align_start = std::chrono::system_clock::now();
-      ndtraw->align(*aligned,init_guess);
+      if(gbuseanh)
+      {
+        anh_ndt.align(*aligned,init_guess);
+      }
+      else
+        ndtraw->align(*aligned,init_guess);
       align_end = std::chrono::system_clock::now();
 
       if(xTime.elapsed()<90)
@@ -1431,16 +1481,27 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     }
 #endif
 
-      has_converged = ndtraw->hasConverged();
-
-      t = ndtraw->getFinalTransformation();
-      iteration = ndtraw->getFinalNumIteration();
-
-      getFitnessScore_start = std::chrono::system_clock::now();
-      fitness_score = ndtraw->getFitnessScore();
-      getFitnessScore_end = std::chrono::system_clock::now();
+      if(gbuseanh)
+      {
+          has_converged = anh_ndt.hasConverged();
+          t = anh_ndt.getFinalTransformation();
+          iteration = anh_ndt.getFinalNumIteration();
+          getFitnessScore_start = std::chrono::system_clock::now();
+          fitness_score = anh_ndt.getFitnessScore();
+          getFitnessScore_end = std::chrono::system_clock::now();
+          trans_probability = anh_ndt.getTransformationProbability();
+      }
+      else
+      {
+          has_converged = ndtraw->hasConverged();
+          t = ndtraw->getFinalTransformation();
+          iteration = ndtraw->getFinalNumIteration();
+          getFitnessScore_start = std::chrono::system_clock::now();
+          fitness_score = ndtraw->getFitnessScore();
+          getFitnessScore_end = std::chrono::system_clock::now();
+          trans_probability = ndtraw->getTransformationProbability();
+      }
 
-      trans_probability = ndtraw->getTransformationProbability();
 
       std::cout<<"score: "<<fitness_score<<" trans_probability:"<<trans_probability<<std::endl;
   //    std::cout<<" scoure is "<<fitness_score<<std::endl;

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.pro

@@ -7,7 +7,7 @@ QMAKE_CXXFLAGS += -std=gnu++17
 
 QMAKE_LFLAGS += -no-pie
 
-QMAKE_CXXFLAGS +=  -g
+#QMAKE_CXXFLAGS +=  -g
 
 # The following define makes your compiler emit warnings if you use
 # any feature of Qt which as been marked deprecated (the exact warnings

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.xml

@@ -0,0 +1,13 @@
+<xml>	
+	<node name="detection_ndt_pclomp.xml">
+		<param name="lidarmsg" value="lidarpc_center" />
+		<param name="gpsmsg" value="ins550d_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.0" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+	</node>
+</xml>

src/detection/detection_ndt_pclomp/main.cpp

@@ -503,13 +503,13 @@ void exitfunc()
 
 int main(int argc, char *argv[])
 {
-    showversion("detection_ndt_matching_gpu_multi");
+    showversion("detection_ndt_pclomp");
     QCoreApplication a(argc, argv);
 
     QString strpath = QCoreApplication::applicationDirPath();
 
    if(argc < 2)
-       strpath = strpath + "/detection_ndt_matching_gpu_multi.xml";
+       strpath = strpath + "/detection_ndt_pclomp.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

src/tool/adcndtmultimapping/adcndtmultimapping.pro

@@ -21,6 +21,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 QMAKE_LFLAGS += -no-pie
 
+QMAKE_CXXFLAGS += -O3
+
 # You can also make your code fail to compile if you use deprecated APIs.
 # In order to do so, uncomment the following line.
 # You can also select to disable deprecated APIs only up to a certain version of Qt.

src/tool/adcndtmultimapping/mainwindow.cpp

@@ -244,19 +244,36 @@ void MainWindow::on_pushButton_Load_clicked()
     mbCalcCalib_Roll = false;
 
     int ncalindex = mpCB_Mode->currentIndex();
-    if(ncalindex == 0)
-    {
+    switch (ncalindex) {
+    case 0:
+        mndtthread.setusendt();
+        break;
+    case 1:
         mndtthread.setusecpu();
+        break;
+    case 2:
+        mndtthread.setuseomp();
+        break;
+    case 3:
+        mndtthread.setusegpu();
+        break;
+    default:
+        mndtthread.setusendt();
+        break;
     }
-    else
-    {
-        if(ncalindex == 1)
-        {
-            mndtthread.setuseomp();
-        }
-        else
-            mndtthread.setusegpu();
-    }
+//    if(ncalindex == 0)
+//    {
+//        mndtthread.setusendt();
+//    }
+//    else
+//    {
+//        if(ncalindex == 1)
+//        {
+//            mndtthread.setuseomp();
+//        }
+//        else
+//            mndtthread.setusegpu();
+//    }
     //获取文件路径
     QString str = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)"));
     if(str.isEmpty())return;
@@ -1078,13 +1095,15 @@ void MainWindow::CreateParamView(QGridLayout *gl)
     pl->setText("Mode");
     pl->setFixedWidth(200);
     QComboBox * pcb = new QComboBox(this);
-    pcb->addItem("CPU");
+    pcb->addItem("NDT");
+    pcb->addItem("ANH");
     pcb->addItem("OpenMP");
 
 #ifdef CUDA_FOUND
     pcb->addItem("GPU");
 #endif
     pcb->setFixedWidth(100);
+    pcb->setCurrentIndex(0);
     gl->addWidget(pl,i,0);
     gl->addWidget(pcb,i,1);
     i++;

src/tool/adcndtmultimapping/ndt_mapping.cpp

@@ -216,11 +216,32 @@ extern bool gbOldLidar;
 
 void setmethod(int nmethod)
 {
-    _method_type = MethodType::PCL_GENERIC;
-    if(nmethod == 2)
+
+    switch(nmethod)
+    {
+    case 0:
+        _method_type = MethodType::PCL_GENERIC;
+        break;
+    case 1:
+        _method_type =   MethodType::PCL_ANH ;
+        break;
+    case 2:
+         _method_type = MethodType::PCL_OPENMP;
+        break;
+    case 3:
         _method_type = MethodType::PCL_ANH_GPU;
-    else
-        _method_type = MethodType::PCL_OPENMP;
+        break;
+    default:
+        _method_type = MethodType::PCL_GENERIC;
+        break;
+    }
+
+//    _method_type =   MethodType::PCL_ANH ;// MethodType::PCL_GENERIC;
+//    if(nmethod == 0)return;
+//    if(nmethod == 2)
+//        _method_type = MethodType::PCL_ANH_GPU;
+//    else
+//        _method_type = MethodType::PCL_OPENMP;
 
 }
 
@@ -723,6 +744,7 @@ void point_ndtmapping(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan)
   }
   else if (_method_type == MethodType::PCL_ANH)
   {
+      std::cout<<"use anh."<<std::endl;
     anh_ndt.setTransformationEpsilon(trans_eps);
     anh_ndt.setStepSize(step_size);
     anh_ndt.setResolution(ndt_res);

src/tool/adcndtmultimapping/ndt_mapping.h

@@ -101,15 +101,19 @@ public:
 
     void setusegpu()
     {
-        setmethod(2);
+        setmethod(3);
     }
     void setusecpu()
     {
-        setmethod(0);
+        setmethod(1);
     }
     void setuseomp()
     {
-        setmethod(1);
+        setmethod(2);
+    }
+    void setusendt()
+    {
+        setmethod(0);
     }
     void run()
     {