change modulecomm. change bqev_multilidarcalib for save yaml file.

src/common/modulecomm/shm/procsm.cpp

@@ -535,6 +535,11 @@ bool procsm::AttachMem()
                 }
 
             }
+            else
+            {
+                mpinfo = NULL;
+                mphead = NULL;
+            }
 
 
             return false;
@@ -758,6 +763,12 @@ unsigned int procsm::getcurrentnext()
 {
 
     checkasm();
+
+    if(mpinfo == NULL)
+    {
+        std::cout<<"procsm::getcurrentnext attach asm fail.  "<<std::endl;
+        return 0;
+    }
     unsigned int nNext;
     mpASM->lock();
     nNext = mpinfo->mNext;

src/tool/bqev_multilidarcalib/bqev_multilidarcalib.pro

@@ -102,6 +102,14 @@ INCLUDEPATH += /opt/ros/melodic/include
 INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/eigen3
 
+!include(../../../include/ivopencv.pri ) {
+    error( "Couldn't find the ivopencv.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
 
 
 INCLUDEPATH += $$PWD/../../../include/

src/tool/bqev_multilidarcalib/mainwindow.cpp

@@ -1,10 +1,16 @@
 #include "mainwindow.h"
 #include "ui_mainwindow.h"
 
+#include <QMessageBox>
+#include <QFileDialog>
+
 #include "ndt_calib.h"
 
 #include "yaml-cpp/yaml.h"
 
+#include <opencv2/opencv.hpp>
+//#include <opencv2/highgui.h">
+
 MainWindow * gw;
 
 
@@ -941,9 +947,71 @@ void MainWindow::onNDTCalc()
 
         }
 
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr merge_pc(new pcl::PointCloud<pcl::PointXYZI>());
+    Eigen::Matrix4f t_calib(Eigen::Matrix4f::Identity());
+
     point_ndtcalib(mLidarData.at(0).mpoint_cloud,mLidarData.at(1).mpoint_cloud,
-                   mfNDT[0],mfNDT[2],mfNDT[4],mfNDT[1],mfNDT[3],mfNDT[5]);
+                   mfNDT[0],mfNDT[2],mfNDT[4],mfNDT[1],mfNDT[3],mfNDT[5],merge_pc,
+            t_calib);
 
     int i;
     for(i=0;i<6;i++)mpLENDT[i]->setText(QString::number(mfNDT[i]));
+
+    std::cout<<" t_calib "<<t_calib<<std::endl;
+
+    QMessageBox::StandardButton button;
+    button=QMessageBox::question(this,tr("Save Merge PointCloud"),QString(tr("Save Merge PointCloud？")),QMessageBox::Yes|QMessageBox::No);
+    if(button==QMessageBox::No)
+    {
+
+    }
+    else if(button==QMessageBox::Yes)
+    {
+        QString str = QFileDialog::getSaveFileName(this,"Save PCD",".","*.pcd");
+        if(!str.isEmpty())
+        {
+            if(str.right(4) == ".pcd")
+            {
+
+            }
+            else
+            {
+                std::cout<<" append .pcd."<<std::endl;
+                str = str + ".pcd";
+            }
+            pcl::io::savePCDFile(str.toStdString(),*merge_pc);
+        }
+    }
+
+    QString str = QFileDialog::getSaveFileName(this,"Save YAML File",".","*.yaml");
+    if(!str.isEmpty())
+    {
+        if(str.right(5) == ".yaml")
+        {
+
+        }
+        else
+        {
+            std::cout<<" append .yaml."<<std::endl;
+            str = str + ".yaml";
+        }
+
+        QFile  xFile;
+        xFile.setFileName(str);
+        if(xFile.open(QIODevice::ReadWrite))
+        {
+            char stroutcalib[10000];
+            snprintf(stroutcalib,10000,"Calib: [ %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f ]",
+                     t_calib(0,0), t_calib(0,1), t_calib(0,2), t_calib(0,3),
+                     t_calib(1,0), t_calib(1,1), t_calib(1,2), t_calib(1,3),
+                     t_calib(2,0), t_calib(2,1), t_calib(2,2), t_calib(2,3),
+                     t_calib(3,0), t_calib(3,1), t_calib(3,2), t_calib(3,3));
+            xFile.write(stroutcalib,strnlen(stroutcalib,10000));
+            xFile.close();
+        }
+
+    }
+
+
 }

src/tool/bqev_multilidarcalib/ndt_calib.cpp

@@ -9,6 +9,10 @@
 #include <ndt_cpu/NormalDistributionsTransform.h>
 #include <pcl/registration/ndt.h>
 
+
+
+#include <thread>
+
 //定义结构体，点的六自由度的数据
 struct pose
 {
@@ -31,7 +35,7 @@ static pose previous_pose, guess_pose, guess_pose_imu, guess_pose_odom, guess_po
 static int max_iter = 30;        // Maximum iterations
 static float ndt_res = 1.0;      // Resolution
 static double step_size = 0.1;   // Step size
-static double trans_eps = 0.01;  // Transformation epsilon
+static double trans_eps = 0.001;  // Transformation epsilon
 
 //tf变换的状态值
 static double _tf_x, _tf_y, _tf_z, _tf_roll, _tf_pitch, _tf_yaw;
@@ -42,7 +46,8 @@ static Eigen::Matrix4f tf_btol, tf_ltob;
 static pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;
 
 void point_ndtcalib(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan1,pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan2,
-                    double &x,double & y,double & z,double & yaw,double & pitch,double & roll)
+                    double &x,double & y,double & z,double & yaw,double & pitch,double & roll,pcl::PointCloud<pcl::PointXYZI>::Ptr & merge_pc,
+                    Eigen::Matrix4f & t_calib)
 {
 
     Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
@@ -53,8 +58,8 @@ void point_ndtcalib(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan1,pcl::PointClo
     tf_ltob = tf_btol.inverse();
 
 
-    const double  min_scan_range = 3.0;
-    const double max_scan_range = 30.0;
+    const double  min_scan_range = 6.0;
+    const double max_scan_range = 100.0;
     double r;
     pcl::PointXYZI p;
     pcl::PointCloud<pcl::PointXYZI> tmp, scan1,scan2;
@@ -108,6 +113,7 @@ void point_ndtcalib(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan1,pcl::PointClo
     pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr1(new pcl::PointCloud<pcl::PointXYZI>(scan1));
     pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr2(new pcl::PointCloud<pcl::PointXYZI>(scan2));
 
+    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr2(new pcl::PointCloud<pcl::PointXYZI>());
 
     double voxel_leaf_size = 0.1;
 
@@ -132,6 +138,7 @@ void point_ndtcalib(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan1,pcl::PointClo
       Eigen::AngleAxisf init_rotation_z(0.0, Eigen::Vector3f::UnitZ());
 
       Eigen::Translation3f init_translation(0.0, 0.0, 0.0);
+//      Eigen::Translation3f init_translation(-0.5, 0.0, -0.3);
 
       Eigen::Matrix4f init_guess =
           (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol;
@@ -168,9 +175,14 @@ void point_ndtcalib(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan1,pcl::PointClo
 
       std::cout<<"prob is "<<transformation_probability<<std::endl;
 
+      std::cout<<" t loc "<<t_localizer<<std::endl;
+
       t_base_link = t_localizer * tf_ltob;
     //  执行点云变换，将该帧的点云变换到以初始状态为起点的坐标系下的点云
-   //   pcl::transformPointCloud(*scan_ptr2, *transformed_scan_ptr2, t_localizer);
+      int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count()/1000;
+      pcl::transformPointCloud(*raw_scan2, *transformed_scan_ptr2, t_localizer);
+      int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count()/1000;
+      std::cout<<"transformPointCloud use: "<<(time2 - time1)<<"us"<<std::endl;
     //  取t_localizer和t_base_link的左上角的3*3的矩阵
       tf::Matrix3x3 mat_l, mat_b;
 
@@ -208,4 +220,14 @@ void point_ndtcalib(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan1,pcl::PointClo
       pitch = ndt_pose.pitch;
       roll = ndt_pose.roll;
 
+ //     pcl::io::savePCDFile("/home/nvidia/app/app/trans.pcd",*transformed_scan_ptr2);
+
+
+      *merge_pc = *transformed_scan_ptr2 + *scan_ptr1;
+      t_calib = t_localizer;
+
+      pcl::io::savePCDFile("/home/nvidia/app/app/merge.pcd",*merge_pc);
+
+
+
 }

src/tool/bqev_multilidarcalib/ndt_calib.h

@@ -3,5 +3,6 @@
 
 
 void point_ndtcalib(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan1,pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan2,
-                    double &x,double & y,double & z,double & yaw,double & pitch,double & roll);
+                    double &x,double & y,double & z,double & yaw,double & pitch,double & roll,pcl::PointCloud<pcl::PointXYZI>::Ptr & merge_pc,
+                    Eigen::Matrix4f & t_calib);
 #endif // NDT_CALIB_H