complete fusion_pointcloud_shenlan.

src/common/ivbacktrace/ivbacktrace.cpp

@@ -274,12 +274,12 @@ void out_stack(char *sig)
 
 void RegisterIVBackTrace()
 {
-    signal(SIGHUP, signal_exit);
-    signal(SIGINT, signal_exit);
-    signal(SIGQUIT, signal_exit);
+//    signal(SIGHUP, signal_exit);
+//    signal(SIGINT, signal_exit);
+//    signal(SIGQUIT, signal_exit);
     signal(SIGABRT, signal_exit);
-    signal(SIGKILL, signal_exit);
-    signal(SIGTERM, signal_exit);
+//    signal(SIGKILL, signal_exit);
+//    signal(SIGTERM, signal_exit);
     signal(SIGSEGV, signal_exit);
 }
 

src/common/modulecomm/modulecomm.pro

@@ -27,7 +27,7 @@ QT += dbus
 DEFINES += USEDBUS
 }
 
-
+QMAKE_CXXFLAGS +=  -g
 
 CONFIG += c++11
 

src/common/modulecomm/shm/procsm.cpp

@@ -124,13 +124,17 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
     mpASMPtr = new QSharedMemory(strsmname);
     mpthreadattmon = new std::thread(&procsm::threadAttachMon,this);
 
+//
+
     char strasmname[300];
+    snprintf(strasmname,255,"%s_%lld",strsmname,std::chrono::system_clock::now().time_since_epoch().count());
 
 
     mnMode = nMode;
     if(nMode == ModeWrite)
     {
         int nrtn = CreateASMPTR(strasmname,nBufSize,nMaxPacCount);
+//        int nrtn = CreateASMPTR(mstrsmname,nBufSize,nMaxPacCount);
         if(nrtn<0)
         {
             std::cout<<"CreateASMPTR Fail."<<std::endl;
@@ -143,11 +147,15 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
         return;
     }
 
+
+
 //    mpASM = new QSharedMemory(strsmname);
     mpASM = new QSharedMemory(strasmname);
 
     if(nMode == ModeWrite)
     {
+
+
         strncpy(mmodulemsg_type.mstrmsgidname,strsmname,255);
         mmodulemsg_type.mnBufSize = nBufSize;
         mmodulemsg_type.mnMsgBufCount = nMaxPacCount;
@@ -164,6 +172,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
 #endif
 #endif
 
+        std::cout<<" create asm impl name: "<<strasmname<<std::endl;
         int nrtn = CreateAndAttachASM(strasmname,nBufSize,nMaxPacCount,strsmname);
         if(nrtn <0 )
         {
@@ -172,6 +181,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
             ivstdcolorout(strerr,iv::STDCOLOR_BOLDRED);
         }
 
+
     }
 
     mbInitComplete = true;
@@ -225,7 +235,7 @@ int procsm::CreateASMPTR(char * strasmname,const unsigned int nBufSize,const uns
         }
         else
         {
-            std::cout<<"Attach Fail."<<std::endl;
+            std::cout<<"Attach "<<mstrsmname<<" Attach Fail."<<std::endl;
             return -1;
         }
     }

src/fusion/fusion_pointcloud_shenlan/fusion_pointcloud_shenlan.pro

@@ -1,6 +1,6 @@
 QT -= gui
 
-CONFIG += c++14 console
+CONFIG += c++14 #console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use
@@ -22,6 +22,11 @@ INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/eigen3
 
+#QMAKE_CXXFLAGS +=  -g
+
+#QMAKE_CXXFLAGS +=  -g
+
+
 #LIBS += /usr/lib/aarch64-linux-gnu/libpcl_*.so
 
 win32: LIBS += -LC:/File/PCL_1.8/PCL_1.8.1/lib -lpcl_common_debug\
@@ -87,3 +92,5 @@ unix:LIBS += -lboost_thread -lboost_system
 !include(../../../include/ivpcl.pri ) {
     error( "Couldn't find the ivpcl.pri file!" )
 }
+
+INCLUDEPATH += /usr/include/eigen3

src/fusion/fusion_pointcloud_shenlan/fusion_pointcloud_shenlan.yaml

@@ -1,3 +1,4 @@
+limitside: false  # if true ,use maximm minimin limit left right lidar.
 left:
   memname: lidarpc_left
   maximum:

src/fusion/fusion_pointcloud_shenlan/main.cpp

@@ -16,6 +16,11 @@
 #include <vector>
 #include <thread>
 
+#include <pcl/common/transforms.h>
+
+
+#include <Eigen/Eigen>
+
 #include "lidarmerge.h"
 #include "modulecomm.h"
 #include "ivversion.h"
@@ -34,8 +39,14 @@ static char gstrcentermemename[256];
 static char gstroutmemname[256];
 static void * gpaout;
 
-static bool gbrun;
+static bool gbrun = true;
+
+static Eigen::Matrix4f gleft_calib(Eigen::Matrix4f::Identity());
+static Eigen::Matrix4f gright_calib(Eigen::Matrix4f::Identity());
 
+static bool gbLoadLeftCalib = false;
+static bool gbLoadRightCalib = false;
+static bool gbLimitSide = false; //if true, use yaml max min
 
 static void InitLidarData()
 {
@@ -65,8 +76,65 @@ static void InitLidarData()
 
 }
 
+void dec_calibyaml(const char * stryamlpath,Eigen::Matrix4f & calib,bool & bload)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryamlpath);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load %s error.",stryamlpath);
+        return;
+    }
 
 
+
+    if(config["Calib"])
+    {
+        YAML::Node node_calib = config["Calib"];
+//        bool isscalar = node_calib.IsScalar();
+//        bool ismap = node_calib.IsMap();
+        bool isseq = node_calib.IsSequence();
+        if(isseq)
+        {
+            int size = node_calib.size();
+            int i=0;
+            int j= 0;
+            if(size == 16)
+            {
+                auto node_it = node_calib.begin();
+                for(;node_it != node_calib.end();node_it++)
+                {
+                    std::string strvalue = node_it->as<std::string>();
+ //                   std::cout<<"value:["<<strvalue<<"]"<<std::endl;
+                    calib(i,j) = atof(strvalue.data());
+                    j++;
+                    if(j>=4)
+                    {
+                        i++;
+                        j=0;
+                    }
+                }
+                bload = true;
+            }
+            else
+            {
+                std::cout<<" calib not 16 values. "<<std::endl;
+            }
+        }
+        else
+        {
+            std::cout<<" calib value not seq. "<<std::endl;
+        }
+
+    }
+
+
+
+}
+
 void dec_yaml(const char * stryamlpath)
 {
 
@@ -77,10 +145,18 @@ void dec_yaml(const char * stryamlpath)
     }
     catch(YAML::BadFile e)
     {
-        qDebug("load error.");
+        qDebug("load %s error.",stryamlpath);
         return;
     }
 
+    if(config["limitside"])
+    {
+        std::string strlimit = config["limitside"].as<std::string>();
+        if(strlimit == "true")
+        {
+            gbLimitSide = true;
+        }
+    }
 
     if(config["left"]["memname"])
     {
@@ -131,6 +207,7 @@ void dec_yaml(const char * stryamlpath)
 
 void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
+//    std::cout<<" listen point cloud. "<<std::endl;
     if(nSize <=16)return;
     unsigned int * pHeadSize = (unsigned int *)strdata;
     if(*pHeadSize > nSize)
@@ -213,6 +290,38 @@ void sharepointcloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud,void * pa)
 }
 
 
+void LimitSide(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,iv::lidar_data & xlidardata)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr new_scan(new pcl::PointCloud<pcl::PointXYZI>());
+    new_scan->header = point_cloud->header;
+    new_scan->width = 0;
+
+    pcl::PointXYZI p;
+    for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = point_cloud->begin(); item != point_cloud->end(); item++)
+    {
+      p.x = (double)item->x;
+      if(p.x > xlidardata.fmax_x)continue;
+      if(p.x < xlidardata.fmin_x)continue;
+
+      p.y = (double)item->y;
+      if(p.y > xlidardata.fmax_y)continue;
+      if(p.y < xlidardata.fmin_y)continue;
+
+      p.z = (double)item->z;
+      if(p.z > xlidardata.fmax_z)continue;
+      if(p.z < xlidardata.fmin_z)continue;
+
+      p.intensity = (double)item->intensity;
+
+      new_scan->push_back(p);
+      new_scan->width++;
+
+    }
+
+    point_cloud = new_scan;
+
+}
+
 
 void mergethread()
 {
@@ -225,7 +334,6 @@ void mergethread()
     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_center = NULL;
     while(gbrun)
     {
-
         bool bCenterUpdate = false;
         bool bLeftUpdate = false;
         bool bRightUpdate = false;
@@ -252,26 +360,56 @@ void mergethread()
         glidar_data_center.mmutex.lock();
         if(glidar_data_center.bUpdate)
         {
-            point_cloud_center = glidar_data_left.mpoint_cloud;
-            ncenterupdatetime = glidar_data_left.mupdatetime;
-            glidar_data_left.bUpdate = false;
+            point_cloud_center = glidar_data_center.mpoint_cloud;
+            ncenterupdatetime = glidar_data_center.mupdatetime;
+            glidar_data_center.bUpdate = false;
             bCenterUpdate = true;
         }
-        glidar_data_left.mmutex.unlock();
+        glidar_data_center.mmutex.unlock();
 
-        if(bLeftUpdate)
+        if(bLeftUpdate && gbLoadLeftCalib)
         {
-
+            if(gbLimitSide)
+            {
+                LimitSide(point_cloud_left,glidar_data_left);
+            }
+            pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr2(new pcl::PointCloud<pcl::PointXYZI>());
+            pcl::transformPointCloud(*point_cloud_left, *transformed_scan_ptr2, gleft_calib);
+            point_cloud_left = transformed_scan_ptr2;
         }
-        if(bRightUpdate)
+        if(bRightUpdate && gbLoadRightCalib)
         {
-
+            if(gbLimitSide)
+            {
+                LimitSide(point_cloud_right,glidar_data_right);
+            }
+            pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr2(new pcl::PointCloud<pcl::PointXYZI>());
+            pcl::transformPointCloud(*point_cloud_right, *transformed_scan_ptr2, gright_calib);
+            point_cloud_right = transformed_scan_ptr2;
         }
         //transform
 
         if(bCenterUpdate)
         {
-
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_out(new pcl::PointCloud<pcl::PointXYZI>());
+            point_cloud_out = point_cloud_center;
+            if(gbLoadLeftCalib &&(abs(ncenterupdatetime - nleftupdatetime)<200) &&(point_cloud_left != NULL))
+            {
+                *point_cloud_out = *point_cloud_out + *point_cloud_left;
+            }
+            else
+            {
+                std::cout<<" no left "<<" left calib: "<<gbLoadLeftCalib<<" time diff: "<<(ncenterupdatetime - nleftupdatetime)<<std::endl;
+            }
+            if(gbLoadRightCalib &&(abs(ncenterupdatetime - nrightupdatetime)<200) &&(point_cloud_right != NULL))
+            {
+                *point_cloud_out = *point_cloud_out + *point_cloud_right;
+            }
+            else
+            {
+                std::cout<<" no right "<<" right calib: "<<gbLoadRightCalib<<" time diff: "<<(ncenterupdatetime - nrightupdatetime)<<std::endl;
+            }
+            sharepointcloud(point_cloud_out,gpaout);
         }
         //Merge
 
@@ -289,7 +427,15 @@ int main(int argc, char *argv[])
 
     InitLidarData();
 
+    snprintf(gstrcentermemename,256,"lidar_pc");
+    snprintf(gstroutmemname,256,"lidarpc_fusion");
+
     dec_yaml("fusion_pointcloud_shenlan.yaml");
+    dec_calibyaml("fusion_left.yaml",gleft_calib,gbLoadLeftCalib);
+    dec_calibyaml("fusion_right.yaml",gright_calib,gbLoadRightCalib);
+
+    std::cout<<"left calib: \n[ "<<gleft_calib<<" ]"<<std::endl;
+    std::cout<<"right calib: \n[ "<<gright_calib<<" ]"<<std::endl;
 
     iv::modulecomm::RegisterRecv(glidar_data_left.strmemname,ListenPointCloud);
     iv::modulecomm::RegisterRecv(glidar_data_right.strmemname,ListenPointCloud);
@@ -299,5 +445,7 @@ int main(int argc, char *argv[])
 
     std::thread xthread(mergethread);
 
+    (void)xthread;
+
     return a.exec();
 }

src/tool/bqev_multilidarcalib/mainwindow.cpp

@@ -960,29 +960,7 @@ void MainWindow::onNDTCalc()
 
     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())
@@ -1001,8 +979,9 @@ void MainWindow::onNDTCalc()
         xFile.setFileName(str);
         if(xFile.open(QIODevice::ReadWrite))
         {
+            xFile.resize(0);//clear data.
             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 ]",
+            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),
@@ -1013,5 +992,29 @@ void MainWindow::onNDTCalc()
 
     }
 
+    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 strpcd = QFileDialog::getSaveFileName(this,"Save PCD",".","*.pcd");
+        if(!strpcd.isEmpty())
+        {
+            if(strpcd.right(4) == ".pcd")
+            {
+
+            }
+            else
+            {
+                std::cout<<" append .pcd."<<std::endl;
+                strpcd = strpcd + ".pcd";
+            }
+            pcl::io::savePCDFile(strpcd.toStdString(),*merge_pc);
+        }
+    }
+
 
 }