|
@@ -1,272 +0,0 @@
|
|
|
-#include <pcl/visualization/cloud_viewer.h>
|
|
|
-#include <iostream>
|
|
|
-#include <pcl/io/io.h>
|
|
|
-#include <pcl/io/pcd_io.h>
|
|
|
-#include <QCoreApplication>
|
|
|
-
|
|
|
-#include "object.pb.h"
|
|
|
-#include "objectarray.pb.h"
|
|
|
-
|
|
|
-#include <thread>
|
|
|
-#include <chrono>
|
|
|
-
|
|
|
-
|
|
|
-#include "modulecomm.h"
|
|
|
-#include "cluster.h"
|
|
|
-
|
|
|
-#include <getopt.h>
|
|
|
-#include <string>
|
|
|
-#include <eigen3/Eigen/Eigen>
|
|
|
-using namespace Eigen;
|
|
|
-using namespace std;
|
|
|
-
|
|
|
-char gstr_memname[256];
|
|
|
-
|
|
|
-
|
|
|
-void print_useage()
|
|
|
-{
|
|
|
- std::cout<<" -m --memname $mappath : share memory name. eq. -m lidar_pc"<<std::endl;
|
|
|
- std::cout<<" -h --help print help"<<std::endl;
|
|
|
-}
|
|
|
-
|
|
|
-std::vector<Obj> Objresult;
|
|
|
-
|
|
|
-void Listenlidarcnndectect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
-{
|
|
|
-
|
|
|
-// std::vector<iv::lidar::object> lidarobjvec;
|
|
|
-// strtolidarobj(lidarobjvec,strdata,nSize);
|
|
|
-
|
|
|
- iv::lidar::objectarray lidarobjvec;
|
|
|
- std::string in;
|
|
|
- in.append(strdata,nSize);
|
|
|
- lidarobjvec.ParseFromString(in);
|
|
|
-
|
|
|
- std::cout<<"lidar obj num: "<<lidarobjvec.obj_size()<<std::endl;
|
|
|
- Obj result;
|
|
|
- Objresult.clear();
|
|
|
- for(int i=0; i<lidarobjvec.obj_size();i++){
|
|
|
- iv::lidar::lidarobject lidarobj = lidarobjvec.obj(i);
|
|
|
- result.center_x = lidarobj.position().x();
|
|
|
- result.center_y = lidarobj.position().y();
|
|
|
- result.center_z = lidarobj.position().z();
|
|
|
- result.l = lidarobj.dimensions().x();
|
|
|
- result.w = lidarobj.dimensions().y();
|
|
|
- result.h = lidarobj.dimensions().z();
|
|
|
- result.yaw = lidarobj.tyaw();
|
|
|
- result.type_name = lidarobj.type_name();
|
|
|
-
|
|
|
- Eigen::AngleAxisf rotation_vector(-lidarobj.tyaw(), Eigen::Vector3f::UnitZ());
|
|
|
- result.Quaternion = Eigen::Quaternionf(rotation_vector);
|
|
|
- Objresult.push_back(result);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-int GetOptLong(int argc, char *argv[]) {
|
|
|
- int nRtn = 0;
|
|
|
- int opt; // getopt_long() 的返回值
|
|
|
- int digit_optind = 0; // 设置短参数类型及是否需要参数
|
|
|
-
|
|
|
- // 如果option_index非空,它指向的变量将记录当前找到参数符合long_opts里的
|
|
|
- // 第几个元素的描述,即是long_opts的下标值
|
|
|
- int option_index = 0;
|
|
|
- // 设置短参数类型及是否需要参数
|
|
|
- const char *optstring = "m:h";
|
|
|
-
|
|
|
- // 设置长参数类型及其简写,比如 --reqarg <==>-r
|
|
|
- /*
|
|
|
- struct option {
|
|
|
- const char * name; // 参数的名称
|
|
|
- int has_arg; // 是否带参数值,有三种:no_argument, required_argument,optional_argument
|
|
|
- int * flag; // 为空时,函数直接将 val 的数值从getopt_long的返回值返回出去,
|
|
|
- // 当非空时,val的值会被赋到 flag 指向的整型数中,而函数返回值为0
|
|
|
- int val; // 用于指定函数找到该选项时的返回值,或者当flag非空时指定flag指向的数据的值
|
|
|
- };
|
|
|
- 其中:
|
|
|
- no_argument(即0),表明这个长参数不带参数(即不带数值,如:--name)
|
|
|
- required_argument(即1),表明这个长参数必须带参数(即必须带数值,如:--name Bob)
|
|
|
- optional_argument(即2),表明这个长参数后面带的参数是可选的,(即--name和--name Bob均可)
|
|
|
- */
|
|
|
- static struct option long_options[] = {
|
|
|
- {"memname", required_argument, NULL, 'm'},
|
|
|
- {"help", no_argument, NULL, 'h'},
|
|
|
- // {"optarg", optional_argument, NULL, 'o'},
|
|
|
- {0, 0, 0, 0} // 添加 {0, 0, 0, 0} 是为了防止输入空值
|
|
|
- };
|
|
|
-
|
|
|
- while ( (opt = getopt_long(argc,
|
|
|
- argv,
|
|
|
- optstring,
|
|
|
- long_options,
|
|
|
- &option_index)) != -1) {
|
|
|
-// printf("opt = %c\n", opt); // 命令参数,亦即 -a -b -n -r
|
|
|
-// printf("optarg = %s\n", optarg); // 参数内容
|
|
|
-// printf("optind = %d\n", optind); // 下一个被处理的下标值
|
|
|
-// printf("argv[optind - 1] = %s\n", argv[optind - 1]); // 参数内容
|
|
|
-// printf("option_index = %d\n", option_index); // 当前打印参数的下标值
|
|
|
-// printf("\n");
|
|
|
- switch(opt)
|
|
|
- {
|
|
|
- case 'm':
|
|
|
- strncpy(gstr_memname,optarg,255);
|
|
|
- break;
|
|
|
- case 'h':
|
|
|
- print_useage();
|
|
|
- nRtn = 1; //because use -h
|
|
|
- break;
|
|
|
- default:
|
|
|
- break;
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- return nRtn;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-pcl::visualization::CloudViewer viewer("Cloud Viewer");//创建viewer对象
|
|
|
-
|
|
|
-
|
|
|
-int user_data;
|
|
|
-
|
|
|
-// // read source lidar_pointcloud
|
|
|
-//void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
-//{
|
|
|
-// 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++;
|
|
|
-
|
|
|
-// }
|
|
|
-
|
|
|
-// user_data++;
|
|
|
-// viewer.showCloud(point_cloud);
|
|
|
-//}
|
|
|
-
|
|
|
-// read segmentation_cnn output lidar_pointcloud
|
|
|
-void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
-{
|
|
|
- iv::lidar::objectarray lidarobjvec;
|
|
|
- std::string in;
|
|
|
- in.append(strdata,nSize);
|
|
|
- lidarobjvec.ParseFromString(in);
|
|
|
-
|
|
|
- pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
|
|
|
- new pcl::PointCloud<pcl::PointXYZI>());
|
|
|
- for(int i=0; i<lidarobjvec.obj_size();i++){
|
|
|
- iv::lidar::lidarobject lidarobj = lidarobjvec.obj(i);
|
|
|
- if(lidarobj.type_name()!="car" && lidarobj.type_name()!="pedestrian")
|
|
|
- continue;
|
|
|
- for(int j=0;j<lidarobj.cloud_size();j++){
|
|
|
-// iv::lidar::PointXYZI Point = lidarobj.cloud(j);
|
|
|
- pcl::PointXYZI xp;
|
|
|
- xp.x = lidarobj.cloud(j).x();
|
|
|
- xp.y = lidarobj.cloud(j).y();
|
|
|
- xp.z = lidarobj.cloud(j).z();
|
|
|
- xp.intensity = lidarobj.cloud(j).i();
|
|
|
- point_cloud->push_back(xp);
|
|
|
- }
|
|
|
- }
|
|
|
- viewer.showCloud(point_cloud);
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
|
|
|
-{
|
|
|
-
|
|
|
- viewer.setBackgroundColor(0.0,0.0,0.0);
|
|
|
-
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
|
|
|
-{
|
|
|
- static unsigned count = 0;
|
|
|
- std::stringstream ss;
|
|
|
-
|
|
|
- ss << "Point Cloud Count: " << user_data;
|
|
|
- viewer.removeShape ("text", 0);
|
|
|
- viewer.addText (ss.str(), 200, 300, "text", 0);
|
|
|
-
|
|
|
- viewer.removeAllShapes();
|
|
|
- viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
|
|
|
-
|
|
|
- for(int i=0;i<Objresult.size();i++){
|
|
|
-
|
|
|
- Eigen::Vector3f center(Objresult[i].center_x,Objresult[i].center_y,Objresult[i].center_z);
|
|
|
- std::string cube = "box"+std::to_string(i);
|
|
|
- viewer.addCube(center, Objresult[i].Quaternion,Objresult[i].w,Objresult[i].h,Objresult[i].l, cube);
|
|
|
- viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube);
|
|
|
- viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, cube);
|
|
|
- viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1, cube);
|
|
|
- std::string cubeFill = "boxFill"+std::to_string(i);
|
|
|
- viewer.addCube(center, Objresult[i].Quaternion,Objresult[i].w,Objresult[i].h,Objresult[i].l, cubeFill);
|
|
|
- viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);
|
|
|
- viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, cubeFill);
|
|
|
- viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, cubeFill);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-int main(int argc, char *argv[])
|
|
|
-{
|
|
|
- QCoreApplication a(argc, argv);
|
|
|
- snprintf(gstr_memname,255,"lidar_pointpillar");
|
|
|
-
|
|
|
- int nRtn = GetOptLong(argc,argv);
|
|
|
- if(nRtn == 1) //show help,so exit.
|
|
|
- {
|
|
|
- return 0;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- //This will only get called once
|
|
|
- viewer.runOnVisualizationThreadOnce (viewerOneOff);
|
|
|
-
|
|
|
- //This will get called once per visualization iteration
|
|
|
- viewer.runOnVisualizationThread (viewerPsycho);
|
|
|
-
|
|
|
- //std::string gstrinput = "lidar_pointpillar";
|
|
|
- std::string gstrinput = "lidar_track";
|
|
|
- void * pa = iv::modulecomm::RegisterRecv(gstr_memname,ListenPointCloud);
|
|
|
- void * gpadetect = iv::modulecomm::RegisterRecv(gstrinput.data(),Listenlidarcnndectect);
|
|
|
-
|
|
|
- while(!viewer.wasStopped())
|
|
|
- {
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
- }
|
|
|
- return 0;
|
|
|
-
|
|
|
-
|
|
|
- return a.exec();
|
|
|
-
|
|
|
-}
|
|
|
-
|