change for hapo lidar fusion.

src/driver/driver_lidar_rs16/lidar_driver_rs16.cpp

@@ -54,6 +54,10 @@ extern char gstr_inclinationang_xaxis[256];  //from x axis
 extern char gstr_hostip[256];
 extern char gstr_port[256];
 
+extern double gfx_off = 0.0;
+extern double gfy_off = 0.0;
+extern double gfz_off = 0.0;
+extern bool gboffset;
 
 class rs16_Buf
 {
@@ -338,6 +342,12 @@ void process_rs16obs(char * strdata,int nLen)
                                 point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
                             }
                             point.intensity = intensity;
+                            if(gboffset)
+                            {
+                                point.x = point.x + gfx_off;
+                                point.y = point.y + gfy_off;
+                                point.z = point.z + gfz_off;
+                            }
                             point_cloud->points.push_back(point);
 
 
@@ -379,6 +389,12 @@ void process_rs16obs(char * strdata,int nLen)
                                 point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
                             }
                             point.intensity = intensity;
+                            if(gboffset)
+                            {
+                                point.x = point.x + gfx_off;
+                                point.y = point.y + gfy_off;
+                                point.z = point.z + gfz_off;
+                            }
                             point_cloud->points.push_back(point);
 
 

src/driver/driver_lidar_rs16/main.cpp

@@ -19,6 +19,10 @@ char gstr_inclinationang_xaxis[256];  //from x axis
 char gstr_hostip[256];
 char gstr_port[256];
 char gstr_yaml[256];
+double gfx_off = 0.0;
+double gfy_off = 0.0;
+double gfz_off = 0.0;
+bool gboffset = false;
 
 
 void print_useage()
@@ -156,6 +160,29 @@ void decodeyaml(const char * stryaml)
     {
         strncpy(gstr_port,config["port"].as<std::string>().data(),255);
     }
+    if(config["offset_x"])
+    {
+        char stroff[256];
+        strncpy(stroff,config["offset_x"].as<std::string>().data(),255);
+        gfx_off = atof(stroff);
+    }
+    if(config["offset_y"])
+    {
+        char stroff[256];
+        strncpy(stroff,config["offset_y"].as<std::string>().data(),255);
+        gfy_off = atof(stroff);
+    }
+    if(config["offset_z"])
+    {
+        char stroff[256];
+        strncpy(stroff,config["offset_z"].as<std::string>().data(),255);
+        gfz_off = atof(stroff);
+    }
+
+    if((fabs(gfx_off>0.001))||(fabs(gfy_off>0.001))||(fabs(gfz_off>0.001)))
+    {
+        gboffset = true;
+    }
 
 
 

src/fusion/fusion_pointcloud_hapo/fusion_pointcloud_hapo.pro

@@ -0,0 +1,96 @@
+QT -= gui
+
+CONFIG += c++14 #console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+QMAKE_LFLAGS += -no-pie
+
+# 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.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    lidarmerge.cpp
+
+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\
+        -lpcl_features_debug\
+        -lpcl_filters_debug\
+        -lpcl_io_debug\
+        -lpcl_io_ply_debug\
+        -lpcl_kdtree_debug\
+        -lpcl_keypoints_debug\
+        -lpcl_octree_debug\
+        -lpcl_outofcore_debug\
+        -lpcl_people_debug\
+        -lpcl_recognition_debug\
+        -lpcl_registration_debug\
+        -lpcl_sample_consensus_debug\
+        -lpcl_search_debug\
+        -lpcl_segmentation_debug\
+        -lpcl_surface_debug\
+        -lpcl_tracking_debug\
+        -lpcl_visualization_debug
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+
+HEADERS += \
+    lidarmerge.h
+
+
+unix:LIBS += -lboost_thread -lboost_system
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+INCLUDEPATH += /usr/include/eigen3

src/fusion/fusion_pointcloud_hapo/fusion_pointcloud_hapo.yaml

@@ -0,0 +1,22 @@
+limitside: false  # if true ,use maximm minimin limit left right lidar.
+left:
+  memname: lidarpc_left
+  maximum:
+    x: 100
+    y: 100.0
+    z: 100
+  minimum:
+    x: -100
+    y: -100.0
+    z: -100
+right:
+  memname: lidarpc_right
+  maximum:
+    x: 100
+    y: 100.0
+    z: 100
+  minimum:
+    x: -100
+    y: -100.0
+    z: -100
+output: lidar_pc

src/fusion/fusion_pointcloud_hapo/lidarmerge.cpp

@@ -0,0 +1,74 @@
+#include "lidarmerge.h"
+
+#include <QDateTime>
+
+#include <QMutex>
+
+extern QMutex gMutex;
+
+static int g_seq = 0;
+
+pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvectorlidar)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = QDateTime::currentMSecsSinceEpoch();
+    point_cloud->width = 0;
+    point_cloud->header.seq =g_seq;
+    g_seq++;
+
+
+    int i,j;
+    for(i=0;i<xvectorlidar.size();i++)
+    {
+        int nsize;
+
+        pcl::PointCloud<pcl::PointXYZI>::Ptr partpoint_cloud;
+        int64_t curtime = QDateTime::currentMSecsSinceEpoch();
+        if((xvectorlidar[i].bUpdate)||((curtime -xvectorlidar[i].mupdatetime)<150))
+        {
+            gMutex.lock();
+            partpoint_cloud = xvectorlidar[i].mpoint_cloud;
+            gMutex.unlock();
+            nsize = partpoint_cloud->width;
+            qDebug("size is %d ",nsize);
+
+            for(j=0;j<nsize;j++)
+            {
+                pcl::PointXYZI point;
+                point = partpoint_cloud->at(j);
+                point.x = (point.x + xvectorlidar[i].foff_x)*cos(xvectorlidar[i].foff_angle) -(point.y + xvectorlidar[i].foff_y)*sin(xvectorlidar[i].foff_angle) ;
+                point.y = (point.y + xvectorlidar[i].foff_y)*cos(xvectorlidar[i].foff_angle) + (point.x + xvectorlidar[i].foff_x)*sin(xvectorlidar[i].foff_angle);
+                point.z = point.z + xvectorlidar[i].foff_z;
+
+                if(point.x>xvectorlidar[i].fmax_x)continue;
+                if(point.y>xvectorlidar[i].fmax_y)continue;
+                if(point.z>xvectorlidar[i].fmax_z)continue;
+                if(point.z > xvectorlidar[i].fignore_zmax) continue;
+                if(point.z < xvectorlidar[i].fignore_zmin) continue;
+//                std::cout<<"  fignore  zmin  "<<xvectorlidar[i].fignore_zmin<<std::endl;
+
+                if(point.x<xvectorlidar[i].fmin_x)continue;
+                if(point.y<xvectorlidar[i].fmin_y)continue;
+                if(point.z<xvectorlidar[i].fmin_z)continue;
+
+                if((point.x>xvectorlidar[i].fignore_xmin)&&(point.x<xvectorlidar[i].fignore_xmax)&&(point.y>xvectorlidar[i].fignore_ymin)&&(point.y<xvectorlidar[i].fignore_ymax))
+                {
+                    continue;
+                }
+    //            if(!pcl_isfinite(point.x)||!pcl_isfinite(point.y)||!pcl_isfinite(point.z)||!pcl_isfinite(point.intensity)) continue;
+
+                point_cloud->push_back(point);
+                ++point_cloud->width;
+            }
+
+
+        }
+    }
+    qDebug("merge size is %d ",point_cloud->width);
+
+    return point_cloud;
+}

src/fusion/fusion_pointcloud_hapo/lidarmerge.h

@@ -0,0 +1,46 @@
+#ifndef LIDARMERGE_H
+#define LIDARMERGE_H
+
+#include <pcl/common/io.h>
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+
+#include <mutex>
+
+
+
+namespace  iv {
+struct lidar_data
+{
+    std::mutex mmutex;
+    char strmemname[256];
+
+
+
+    double foff_x = 0.0;
+    double foff_y = 0.0;
+    double foff_z = 0.0;
+    double foff_angle = 0.0;
+    bool bUpdate = false;
+    double fmax_x = 0;
+    double fmax_y = 0;
+    double fmax_z = 0;
+    double fmin_x = 0;
+    double fmin_y = 0;
+    double fmin_z = 0;
+    double fignore_xmax = 0;
+    double fignore_ymax = 0;
+    double fignore_xmin = 0;
+    double fignore_ymin = 0;
+    double fignore_zmax = 0;
+    double fignore_zmin = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud = NULL;
+    int64_t mupdatetime = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_old = NULL;
+};
+
+}
+
+pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvectorlidar);
+#endif // LIDARMERGE_H

src/fusion/fusion_pointcloud_hapo/main.cpp

@@ -0,0 +1,429 @@
+#include <QCoreApplication>
+#include <QDateTime>
+
+#include <pcl/common/io.h>
+
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+//#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/radius_outlier_removal.h>
+
+#include <iostream>
+#include <fstream>
+#include <yaml-cpp/yaml.h>
+#include <QMutex>
+#include <vector>
+#include <thread>
+
+#include <pcl/common/transforms.h>
+
+
+#include <Eigen/Eigen>
+
+#include "lidarmerge.h"
+#include "modulecomm.h"
+#include "ivversion.h"
+
+
+QMutex gMutex;
+
+static qint64 glasttime = 0;
+static std::vector<iv::lidar_data> gvectorlidar;
+
+static iv::lidar_data glidar_data_left;
+static iv::lidar_data glidar_data_right;
+static iv::lidar_data glidar_data_center;
+
+static char gstrcentermemename[256];
+static char gstroutmemname[256];
+static void * gpaout;
+
+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()
+{
+    snprintf(glidar_data_left.strmemname,256,"lidarpc_left");
+    snprintf(glidar_data_right.strmemname,256,"lidarpc_right");
+
+    glidar_data_left.fmax_x = 300;
+    glidar_data_left.fmax_y = 300;
+    glidar_data_left.fmax_z = 300;
+    glidar_data_left.fmin_x = -300;
+    glidar_data_left.fmin_y = -300;
+    glidar_data_left.fmin_z = -300;
+    glidar_data_left.bUpdate = false;
+    glidar_data_left.mupdatetime = 0;
+
+    glidar_data_right.fmax_x = 300;
+    glidar_data_right.fmax_y = 300;
+    glidar_data_right.fmax_z = 300;
+    glidar_data_right.fmin_x = -300;
+    glidar_data_right.fmin_y = -300;
+    glidar_data_right.fmin_z = -300;
+    glidar_data_right.bUpdate = false;
+    glidar_data_right.mupdatetime = 0;
+
+    glidar_data_center.bUpdate = false;
+    glidar_data_center.mupdatetime = 0;
+
+}
+
+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)
+{
+
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryamlpath);
+    }
+    catch(YAML::BadFile e)
+    {
+        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"])
+    {
+        snprintf(glidar_data_left.strmemname,256,"%s",config["left"]["memname"].as<std::string>().data());
+    }
+    if(config["left"]["maximum"]["x"])glidar_data_left.fmax_x = atof(config["left"]["maximum"]["x"].as<std::string>().data());
+    if(config["left"]["maximum"]["y"])glidar_data_left.fmax_y = atof(config["left"]["maximum"]["y"].as<std::string>().data());
+    if(config["left"]["maximum"]["z"])glidar_data_left.fmax_z = atof(config["left"]["maximum"]["z"].as<std::string>().data());
+    if(config["left"]["minimum"]["x"])glidar_data_left.fmin_x = atof(config["left"]["minimum"]["x"].as<std::string>().data());
+    if(config["left"]["minimum"]["y"])glidar_data_left.fmin_y = atof(config["left"]["minimum"]["y"].as<std::string>().data());
+    if(config["left"]["minimum"]["z"])glidar_data_left.fmin_z = atof(config["left"]["minimum"]["z"].as<std::string>().data());
+
+    if(config["right"]["memname"])
+    {
+        snprintf(glidar_data_right.strmemname,256,"%s",config["right"]["memname"].as<std::string>().data());
+    }
+    if(config["right"]["maximum"]["x"])glidar_data_right.fmax_x = atof(config["right"]["maximum"]["x"].as<std::string>().data());
+    if(config["right"]["maximum"]["y"])glidar_data_right.fmax_y = atof(config["right"]["maximum"]["y"].as<std::string>().data());
+    if(config["right"]["maximum"]["z"])glidar_data_right.fmax_z = atof(config["right"]["maximum"]["z"].as<std::string>().data());
+    if(config["right"]["minimum"]["x"])glidar_data_right.fmin_x = atof(config["right"]["minimum"]["x"].as<std::string>().data());
+    if(config["right"]["minimum"]["y"])glidar_data_right.fmin_y = atof(config["right"]["minimum"]["y"].as<std::string>().data());
+    if(config["right"]["minimum"]["z"])glidar_data_right.fmin_z = atof(config["right"]["minimum"]["z"].as<std::string>().data());
+
+    if(config["center"]["memname"])
+    {
+        snprintf(gstrcentermemename,256,"%s",config["center"]["memname"].as<std::string>().data());
+    }
+
+
+    if(config["output"])
+    {
+       strncpy(gstroutmemname,config["output"].as<std::string>().data(),255);
+    }
+    else
+    {
+        strncpy(gstroutmemname,"lidar_pc",255);
+    }
+
+    iv::lidar_data * pl = &glidar_data_left;
+    pl = &glidar_data_right;
+
+    char * strmemc = gstrcentermemename;
+    strmemc = gstroutmemname;
+
+    return;
+
+}
+
+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)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        xp.x = p->x;
+        xp.y = p->y;
+        xp.z = p->z;
+        xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+
+  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
+    }
+
+    if(strncmp(strmemname,glidar_data_left.strmemname,255) == 0)
+    {
+        glidar_data_left.mmutex.lock();
+        glidar_data_left.mpoint_cloud = point_cloud;
+        glidar_data_left.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_left.bUpdate = true;
+        glidar_data_left.mmutex.unlock();
+    }
+
+    if(strncmp(strmemname,glidar_data_right.strmemname,255) == 0)
+    {
+        glidar_data_right.mmutex.lock();
+        glidar_data_right.mpoint_cloud = point_cloud;
+        glidar_data_right.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_right.bUpdate = true;
+        glidar_data_right.mmutex.unlock();
+    }
+
+    if(strncmp(strmemname,gstrcentermemename,255) == 0)
+    {
+        glidar_data_center.mmutex.lock();
+        glidar_data_center.mpoint_cloud = point_cloud;
+        glidar_data_center.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_center.bUpdate = true;
+        glidar_data_center.mmutex.unlock();
+    }
+
+}
+
+void sharepointcloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud,void * pa)
+{
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+  //  pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    unsigned int * pu32 = (unsigned int *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+    delete strOut;
+}
+
+
+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()
+{
+    int i;
+    int64_t nleftupdatetime = 0;
+    int64_t nrightupdatetime = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_left = NULL;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_right = NULL;
+    while(gbrun)
+    {
+        bool bLeftUpdate = false;
+        bool bRightUpdate = false;
+        glidar_data_left.mmutex.lock();
+        if(glidar_data_left.bUpdate)
+        {
+            point_cloud_left = glidar_data_left.mpoint_cloud;
+            nleftupdatetime = glidar_data_left.mupdatetime;
+            glidar_data_left.bUpdate = false;
+            bLeftUpdate = true;
+        }
+        glidar_data_left.mmutex.unlock();
+
+        glidar_data_right.mmutex.lock();
+        if(glidar_data_right.bUpdate)
+        {
+            point_cloud_right = glidar_data_right.mpoint_cloud;
+            nrightupdatetime = glidar_data_right.mupdatetime;
+            glidar_data_right.bUpdate = false;
+            bRightUpdate = true;
+        }
+        glidar_data_right.mmutex.unlock();
+
+
+
+        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 && 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(bLeftUpdate)
+        {
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_out(new pcl::PointCloud<pcl::PointXYZI>());
+            point_cloud_out = point_cloud_left;
+
+            if(gbLoadRightCalib &&(abs(nleftupdatetime - 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: "<<(nleftupdatetime - nrightupdatetime)<<std::endl;
+            }
+            sharepointcloud(point_cloud_out,gpaout);
+        }
+        //Merge
+
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+    }
+}
+
+
+int main(int argc, char *argv[])
+{
+    showversion("fusion_pointcloud_hapo");
+    QCoreApplication a(argc, argv);
+
+    InitLidarData();
+
+    snprintf(gstroutmemname,256,"lidar_pc");
+
+    dec_yaml("fusion_pointcloud_hapo.yaml");
+    dec_calibyaml("fusion_rightonleft.yaml",gright_calib,gbLoadRightCalib);
+
+    std::cout<<"rightonleft calib: \n[ "<<gright_calib<<" ]"<<std::endl;
+
+    iv::modulecomm::RegisterRecv(glidar_data_left.strmemname,ListenPointCloud);
+    iv::modulecomm::RegisterRecv(glidar_data_right.strmemname,ListenPointCloud);
+
+    gpaout = iv::modulecomm::RegisterSend(gstroutmemname,20000000,1);
+
+    std::thread xthread(mergethread);
+
+    (void)xthread;
+
+    return a.exec();
+}