complete pcl_omp.

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.pro

@@ -56,13 +56,6 @@ INCLUDEPATH += $$PWD/../../include/msgtype/
 
 INCLUDEPATH += $$PWD/pclomp
 
-INCLUDEPATH += /usr/local/cuda-10.0/targets/aarch64-linux/include
-INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include
-INCLUDEPATH += /usr/local/cuda-10.2/targets/x86_64-linux/include
-INCLUDEPATH += /usr/local/cuda/targets/aarch64-linux/include
-
-DEFINES += CUDA_FOUND
-
 DEFINES += _OPENMP
 #DEFINES += USE_PCL_OPENMP
 
@@ -88,7 +81,7 @@ unix:LIBS +=  -lpcl_common\
         -lpcl_visualization
 
 
-INCLUDEPATH += $$PWD/../../common/ndt_gpu/
+#INCLUDEPATH += $$PWD/../../common/ndt_gpu/
 
 
 !include(../../../include/common.pri ) {

src/detection/detection_ndt_pclomp/ndt_matching.cpp

@@ -59,7 +59,7 @@
 
 #include <QFile>
 
-#include <ndt_gpu/NormalDistributionsTransform.h>
+//#include <ndt_gpu/NormalDistributionsTransform.h>
 #include "ndtpos.pb.h"
 #include "ndtgpspos.pb.h"
 
@@ -147,17 +147,13 @@ static int _use_gnss = 1;
 static int init_pos_set = 0;
 
 static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
-//static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
-#ifdef CUDA_FOUND
-static std::shared_ptr<gpu::GNormalDistributionsTransform> anh_gpu_ndt_ptr =
-    std::make_shared<gpu::GNormalDistributionsTransform>();
-#endif
-#ifdef USE_PCL_OPENMP
-static pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> omp_ndt;
-#endif
+
 
 pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
 
+pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+
 // Default values
 static int max_iter = 30;        // Maximum iterations
 static float ndt_res = 1.0;      // Resolution
@@ -304,7 +300,6 @@ static bool gbGFRun = true;
 static bool gbLMRun= true;
 static std::thread * gpmapthread, * gpgpsfixthread,*gpsavelastthread;
 static bool gbNeedUpdate = false;
-static std::shared_ptr<gpu::GNormalDistributionsTransform> glocal_ndt;
 
 extern iv::gpspos glastndtgpspos;
 extern iv::gpspos gcurndtgpspos;
@@ -467,7 +462,6 @@ static void getmindistrace(int & index, double & ftracedis)
     }
 }
 
-std::vector<std::shared_ptr<gpu::GNormalDistributionsTransform>> gvectoranh;
 #include <QTime>
 
 extern void pausecomm();
@@ -493,42 +487,44 @@ static void UseMap(int index)
 
     qDebug("load map time is %d",xTime.elapsed());
 
-    pausecomm();   //Because cuda can pause process, avoid mutex comm, stop this process's comm
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
     pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*xmap));
+//    gvectoranh.push_back(new_anh_gpu_ndt_ptr);
 
-    std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
-        std::make_shared<gpu::GNormalDistributionsTransform>();
-    new_anh_gpu_ndt_ptr->setResolution(ndt_res);
-    new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
-    new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
-    new_anh_gpu_ndt_ptr->setStepSize(step_size);
-    new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+    qDebug("ndt load finish time is %d",xTime.elapsed());
 
-    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
-    pcl::PointXYZ dummy_point;
-    dummy_scan_ptr->push_back(dummy_point);
-    new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);
+    gcurmapindex = index;
+
+    std::cout<<" max threads. "<<omp_get_max_threads()<<std::endl;
+
+    int nthread = omp_get_max_threads();
+    nthread = nthread - 2;
+    if(nthread <1)nthread = 1;
 
-    new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());
+    std::cout<<" use threads: "<<nthread<<std::endl;
+
+    pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+
+
+    localndt_omp->setNumThreads(nthread);
+    localndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
+
+    localndt_omp->setResolution(ndt_res);
+    localndt_omp->setInputTarget(map_ptr);
+    localndt_omp->setMaximumIterations(max_iter);
+    localndt_omp->setStepSize(step_size);
+    localndt_omp->setTransformationEpsilon(trans_eps);
 
     pthread_mutex_lock(&mutex);
-    glocal_ndt = new_anh_gpu_ndt_ptr;
+//        ndt = glocal_ndt;
+    glocalndt_omp = localndt_omp;
     pthread_mutex_unlock(&mutex);
 
-    continuecomm();
-//    gvectoranh.push_back(new_anh_gpu_ndt_ptr);
-
-    qDebug("ndt load finish time is %d",xTime.elapsed());
     gbNeedUpdate = true;
-    gcurmapindex = index;
 
-    ndt_omp->setResolution(ndt_res);
-    ndt_omp->setInputTarget(map_ptr);
-    ndt_omp->setMaximumIterations(max_iter);
-    ndt_omp->setStepSize(step_size);
-    ndt_omp->setTransformationEpsilon(trans_eps);
-    std::cout<<" ndt_omp init success. "<<std::endl;
+
+    std::cout<<" ndt_omp init successfully. "<<std::endl;
 
 
     std::cout<<"change map, index is "<<index<<std::endl;
@@ -740,17 +736,6 @@ void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
     angular_velocity = 0;
 
 
-//    ndt.setResolution(ndt_res);
-//    ndt.setStepSize(step_size);
-//    ndt.setTransformationEpsilon(trans_eps);
-//    ndt.setMaximumIterations(max_iter);
-
-  //  anh_gpu_ndt_ptr->setResolution();
-
-    anh_gpu_ndt_ptr->setResolution(ndt_res);
-    anh_gpu_ndt_ptr->setStepSize(step_size);
-    anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
-    anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
 
 
 //    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
@@ -790,30 +775,22 @@ void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
         return;
     }
 
-    std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
-        std::make_shared<gpu::GNormalDistributionsTransform>();
-    new_anh_gpu_ndt_ptr->setResolution(ndt_res);
-    new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
-    new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
-    new_anh_gpu_ndt_ptr->setStepSize(step_size);
-    new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
     pcl::PointXYZ dummy_point;
     dummy_scan_ptr->push_back(dummy_point);
-    new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);
 
-    new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());
-
-    pthread_mutex_lock(&mutex);
-    anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
-    pthread_mutex_unlock(&mutex);
 
     ndt_omp->setResolution(ndt_res);
     ndt_omp->setInputTarget(map_ptr);
     ndt_omp->setMaximumIterations(max_iter);
     ndt_omp->setStepSize(step_size);
     ndt_omp->setTransformationEpsilon(trans_eps);
+
+    ndt_omp->setNumThreads(6);
+    ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
     std::cout<<" ndt_omp init success. "<<std::endl;
 
 
@@ -883,10 +860,6 @@ void ndt_match_Init_nomap()
     current_velocity_z = 0;
     angular_velocity = 0;
 
-    anh_gpu_ndt_ptr->setResolution(ndt_res);
-    anh_gpu_ndt_ptr->setStepSize(step_size);
-    anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
-    anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
 
     gpmapthread = new std::thread(LocalMapUpdate,1);
     gbGFRun = true;
@@ -906,35 +879,15 @@ void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
     pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
 
 
-    std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
-        std::make_shared<gpu::GNormalDistributionsTransform>();
-    new_anh_gpu_ndt_ptr->setResolution(ndt_res);
-    new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
-    new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
-    new_anh_gpu_ndt_ptr->setStepSize(step_size);
-    new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
     pcl::PointXYZ dummy_point;
     dummy_scan_ptr->push_back(dummy_point);
-    new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);
 
-    new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());
 
-    pthread_mutex_lock(&mutex);
-    anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
-    pthread_mutex_unlock(&mutex);
 
 
-    ndt_omp->setResolution(ndt_res);
-    ndt_omp->setInputTarget(map_ptr);
-    ndt_omp->setMaximumIterations(max_iter);
-    ndt_omp->setStepSize(step_size);
-    ndt_omp->setTransformationEpsilon(trans_eps);
-
-    ndt_omp->setNumThreads(6);
-    ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
-    std::cout<<" ndt_omp init success. "<<std::endl;
 
 
 
@@ -1298,8 +1251,7 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     if(gbNeedUpdate)
     {
         pthread_mutex_lock(&mutex);
-//        ndt = glocal_ndt;
-        anh_gpu_ndt_ptr = glocal_ndt;
+        ndt_omp = glocalndt_omp;
         pthread_mutex_unlock(&mutex);
         gusemapindex = gcurmapindex;
         gbNeedUpdate = false;

src/tool/adcndtmultimapping/adcndtmultimapping.pro

@@ -39,7 +39,9 @@ SOURCES += \
     ivview.cpp \
     myview.cpp \
     ivtraceview.cpp \
-    gnss_coordinate_convert.cpp
+    gnss_coordinate_convert.cpp \
+    ../../detection/detection_ndt_pclomp/pclomp/ndt_omp.cpp \
+    ../../detection/detection_ndt_pclomp/pclomp/voxel_grid_covariance_omp.cpp
 
 HEADERS += \
         mainwindow.h \
@@ -50,7 +52,11 @@ HEADERS += \
     ivview.h \
     myview.h \
     ivtraceview.h \
-    gnss_coordinate_convert.h
+    gnss_coordinate_convert.h \
+    ../../detection/detection_ndt_pclomp/pclomp/ndt_omp_impl.hpp \
+    ../../detection/detection_ndt_pclomp/pclomp/voxel_grid_covariance_omp_impl.hpp \
+    ../../detection/detection_ndt_pclomp/pclomp/pclomp/ndt_omp.h \
+    ../../detection/detection_ndt_pclomp/pclomp/pclomp/voxel_grid_covariance_omp.h
 
 FORMS += \
         mainwindow.ui
@@ -65,6 +71,9 @@ unix:INCLUDEPATH += /usr/include/pcl-1.10
 
 #INCLUDEPATH += /usr/include/qwt
 
+DEFINES += USE_PCL_OPENMP
+DEFINES += _OPENMP
+
 unix:LIBS += -lboost_thread -lboost_system
 
 unix:LIBS +=  -lpcl_common\
@@ -88,6 +97,8 @@ unix:LIBS +=  -lpcl_common\
 
 INCLUDEPATH += $$PWD/../../common/ndt_cpu/
 
+INCLUDEPATH += $$PWD/../../detection/detection_ndt_pclomp/pclomp
+
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -114,3 +125,5 @@ LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog
 
 RESOURCES += \
     adcndtmultimapping.qrc
+
+LIBS += -lmpi -lopen-pal -lgomp

src/tool/adcndtmultimapping/mainwindow.cpp

@@ -250,7 +250,12 @@ void MainWindow::on_pushButton_Load_clicked()
     }
     else
     {
-        mndtthread.setusegpu();
+        if(ncalindex == 1)
+        {
+            mndtthread.setuseomp();
+        }
+        else
+            mndtthread.setusegpu();
     }
     //获取文件路径
     QString str = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)"));
@@ -1074,6 +1079,7 @@ void MainWindow::CreateParamView(QGridLayout *gl)
     pl->setFixedWidth(200);
     QComboBox * pcb = new QComboBox(this);
     pcb->addItem("CPU");
+    pcb->addItem("OpenMP");
 
 #ifdef CUDA_FOUND
     pcb->addItem("GPU");

src/tool/adcndtmultimapping/ndt_mapping.cpp

@@ -47,7 +47,18 @@
 #include <ndt_gpu/NormalDistributionsTransform.h>
 #endif
 #ifdef USE_PCL_OPENMP
-#include <pcl_omp_registration/ndt.h>
+//#include <pcl_omp_registration/ndt.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/ndt.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/filters/voxel_grid.h>
+//#include <pcl/visualization/pcl_visualizer.h>
+
+#include <pclomp/ndt_omp.h>
+
 #endif
 
 #include <ndt_mapping.h>
@@ -109,7 +120,8 @@ static cpu::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> anh_ndt
 static gpu::GNormalDistributionsTransform anh_gpu_ndt;
 #endif
 #ifdef USE_PCL_OPENMP
-static pcl_omp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> omp_ndt;
+//static pcl_omp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> omp_ndt;
+static pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> omp_ndt;
 #endif
 
 
@@ -207,6 +219,9 @@ void setmethod(int nmethod)
     _method_type = MethodType::PCL_GENERIC;
     if(nmethod == 2)
         _method_type = MethodType::PCL_ANH_GPU;
+    else
+        _method_type = MethodType::PCL_OPENMP;
+
 }
 
 static double wrapToPm(double a_num, const double a_max)
@@ -631,6 +646,9 @@ void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double
 void point_ndtmapping(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan)
 {
 
+    omp_ndt.setNumThreads(8);
+    omp_ndt.setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
 //}
 //static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
 //{
@@ -726,6 +744,7 @@ void point_ndtmapping(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan)
 #ifdef USE_PCL_OPENMP
   else if (_method_type == MethodType::PCL_OPENMP)
   {
+      std::cout<<"use ompmp."<<std::endl;
     omp_ndt.setTransformationEpsilon(trans_eps);
     omp_ndt.setStepSize(step_size);
     omp_ndt.setResolution(ndt_res);
@@ -1266,6 +1285,13 @@ void ndt_mapping_Init()
     tp.yaw = current_pose.yaw;
     gsplit_vectortp.push_back(tp);
 
+#ifdef USE_PCL_OPENMP
+
+    omp_ndt.setNumThreads(8);
+    omp_ndt.setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
+#endif
+
 }
 
 #include <QFile>

src/tool/adcndtmultimapping/ndt_mapping.h

@@ -107,6 +107,10 @@ public:
     {
         setmethod(0);
     }
+    void setuseomp()
+    {
+        setmethod(1);
+    }
     void run()
     {