#include "lidarmerge.h" #include #include extern QMutex gMutex; static int g_seq = 0; pcl::PointCloud::Ptr mergefunc(std::vector xvectorlidar) { pcl::PointCloud::Ptr point_cloud( new pcl::PointCloud()); 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>旋转矩阵 rotation_matrix = q_zyx.toRotationMatrix(); trans_matrix << xvectorlidar[i].foff_x, xvectorlidar[i].foff_y, xvectorlidar[i].foff_z;//x,y,z int nsize; pcl::PointCloud::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;jat(j); Eigen::Vector3d new_point, old_point; old_point<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_xmin)&&(point.xxvectorlidar[i].fignore_ymin)&&(point.ypush_back(point); ++point_cloud->width; } } } qDebug("merge size is %d ",point_cloud->width); return point_cloud; }