|
|
@@ -0,0 +1,165 @@
|
|
|
+#include <pcl/visualization/cloud_viewer.h>
|
|
|
+#include <iostream>
|
|
|
+#include <pcl/io/io.h>
|
|
|
+#include <pcl/io/pcd_io.h>
|
|
|
+#include <QCoreApplication>
|
|
|
+#include <QVector>
|
|
|
+#include <thread>
|
|
|
+#include <chrono>
|
|
|
+#include <qjsondocument.h>
|
|
|
+#include <qjsonobject.h>
|
|
|
+#include <QJsonArray>
|
|
|
+#include <QDebug>
|
|
|
+#include <QFile>
|
|
|
+//#include "modulecomm.h"
|
|
|
+#include "mathlocation.h"
|
|
|
+#include <getopt.h>
|
|
|
+
|
|
|
+char gstr_mapname[256];
|
|
|
+char gstr_xmlpath[256];
|
|
|
+char gstr_logpath[256];
|
|
|
+
|
|
|
+void print_useage()
|
|
|
+{
|
|
|
+ std::cout<<" -p --pcd $pcdpath : map path. eq. -p /home/nvidia/map/gpu.pcd"<<std::endl;
|
|
|
+ std::cout<<" -h --help print help"<<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+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 = "p: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[] = {
|
|
|
+ {"pcdpath", required_argument, NULL, 'p'},
|
|
|
+ {"help", no_argument, NULL, 'h'},
|
|
|
+ // {"optarg", optional_argument, NULL, 'o'},
|
|
|
+ {0, 0, 0, 0} // 添加 {0, 0, 0, 0} 是为了防止输入空值
|
|
|
+ };
|
|
|
+ strncpy(gstr_mapname,"/home/nvidia/BorregasAve.pcd",255);
|
|
|
+ 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 'p':
|
|
|
+ strncpy(gstr_mapname,optarg,255);
|
|
|
+ break;
|
|
|
+ case 'h':
|
|
|
+ print_useage();
|
|
|
+ nRtn = 1; //because use -h
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ return nRtn;
|
|
|
+}
|
|
|
+int writeFile(QJsonObject obj)
|
|
|
+{
|
|
|
+ QByteArray baframe;
|
|
|
+ QString filename = "/home/nvidia/pointCloud.json";
|
|
|
+ QFile xFile(filename);
|
|
|
+ if(!xFile.open(QIODevice::QIODevice::WriteOnly))
|
|
|
+ {
|
|
|
+ std::cout<<" File Fail."<<std::endl;
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ baframe = QJsonDocument(obj).toJson();
|
|
|
+ int result = xFile.write(baframe);
|
|
|
+ xFile.close();
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+int main(int argc, char *argv[])
|
|
|
+{
|
|
|
+ QCoreApplication a(argc, argv);
|
|
|
+
|
|
|
+ snprintf(gstr_mapname,255,"");
|
|
|
+
|
|
|
+ int nRtn = GetOptLong(argc,argv);
|
|
|
+ if(nRtn == 1) //show help,so exit.
|
|
|
+ {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
|
|
|
+ new pcl::PointCloud<pcl::PointXYZI>());
|
|
|
+ std::string path = gstr_mapname;
|
|
|
+
|
|
|
+ if(path.length() < 1)
|
|
|
+ {
|
|
|
+ std::cout<<"Please use -p set pcd path."<<std::endl;
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ pcl::io::loadPCDFile<pcl::PointXYZI>(path,*point_cloud);
|
|
|
+
|
|
|
+
|
|
|
+ double lat = 39.12072740;
|
|
|
+ double lon= 117.02800329;
|
|
|
+ double latx,lonx;
|
|
|
+ double x,y;
|
|
|
+ GaussProjCal(lon, lat, &x, &y);
|
|
|
+
|
|
|
+ QJsonArray dataArray,featureArry;
|
|
|
+ QJsonObject dataObj,properObj,geoObj,obj;
|
|
|
+ dataObj.insert("type",QJsonValue("Feature"));
|
|
|
+ properObj.insert("color",QJsonValue("#111111"));
|
|
|
+ dataObj.insert("properties",QJsonValue(properObj));
|
|
|
+ geoObj.insert("type",QJsonValue("MultiPoint"));
|
|
|
+
|
|
|
+ int size = point_cloud->size();
|
|
|
+ for(int i = 0; i < size; i++)
|
|
|
+ {
|
|
|
+ QJsonArray arr;
|
|
|
+ GaussProjInvCal(point_cloud->points[i].x+x, point_cloud->points[i].y+y, &lon, &lat);
|
|
|
+ arr.append(QJsonValue(lon));
|
|
|
+ arr.append(QJsonValue(lat));
|
|
|
+ arr.append(QJsonValue(point_cloud->points[i].z));
|
|
|
+ dataArray.append(QJsonValue(arr));
|
|
|
+ qDebug()<<arr;
|
|
|
+ }
|
|
|
+ geoObj.insert("coordinates",QJsonValue(dataArray));
|
|
|
+ dataObj.insert("geometry",geoObj);
|
|
|
+ featureArry.append(QJsonValue(dataObj));
|
|
|
+ obj.insert("type",QJsonValue("FeatureCollection"));
|
|
|
+ obj.insert("features",QJsonValue(featureArry));
|
|
|
+ int result = writeFile(obj);
|
|
|
+ qDebug("Convert compless, %d point has been writed",result);
|
|
|
+
|
|
|
+ return a.exec();
|
|
|
+}
|
|
|
+
|
lijinliang