把livoxSDK的代码修改一下,添加一个将每一帧内的点云数据逐个数据包保存为本地txt文件的方法:
//每帧点云数据写入txt文件
void WriteTXTFile(std::list<LvxBasePackDetail> point_packet_list_to_save)
{
list<LvxBasePackDetail>::iterator packitr;
packitr = point_packet_list_to_save.begin();//在point packet list temp里迭代所有point packet
int PackNum = 0;
for (packitr = point_packet_list_to_save.begin(); packitr != point_packet_list_to_save.end(); packitr++)
{
//cout << "Packet " << PackNum++ << ":"<<endl;
int RawPoint[100][13] = { 0 };
for (int flag = 0; flag < 1300; flag++)//100个点*13byte空间
{
//cout << int(packitr->raw_point[flag]) << " ";
RawPoint[flag / 13][flag % 13] = packitr->raw_point[flag];//100个点的raw_point数据写入RawPoint二维数组
}
//cout << endl;
int CookedPoint[100][4] = { 0 };
for (int i = 0; i < 100; i++)//uint32_t类型的rawpoint转int坐标
{
CookedPoint[i][0] = Dec4ToBin(RawPoint[i][0], RawPoint[i][1], RawPoint[i][2], RawPoint[i][3]);
CookedPoint[i][1] = Dec4ToBin(RawPoint[i][4], RawPoint[i][5], RawPoint[i][6], RawPoint[i][7]);
CookedPoint[i][2] = Dec4ToBin(RawPoint[i][8], RawPoint[i][9], RawPoint[i][10], RawPoint[i][11]);
CookedPoint[i][3] = RawPoint[i][12];
}
WriteFile(CookedPoint);
}
}
直接调用这个函数,雷达数据获取时间设定为1秒,查看本地文件发现txt文件和lvx文件仅仅保存了56000左右个点的数据。数据获取时间为5秒时,两个文件保存的点的个数大约是28万。正常来讲点云扫描的频率应该是每秒十万个,但是用这种算法保存点云,仅仅保存了一半左右的数据。考虑到将一秒内的1000个数据包的点云保存为txt文件时,耗时大约为4.4秒,可以推断在点云保存txt文件时不知因为何种原因,point_cloud_packet的GetLidar函数部分没有生效。
解决这种问题可以有两种途径,第一种是减少本地保存算法的耗时直到这部分时间消耗可以忽略;另一种是让GetLidar的流程和本地保存的流程能够并行。SDK源码采用mutex互斥锁帮助实现雷达数据获取和lvx文件保存两种流程实现并行,于是想借用一下这部分功能。
新建一个线程,在这个线程里保存txt文件,同时主线程继续获取雷达数据。
thread a (WriteTXTFile,point_packet_list_temp);//写入txt文件
a.detach();
采用这种方法后,点云获取频率可以回到正常水平了。
将保存了相同点云的本地txt文件和lvx文件进行比对,发现两者之间有细微的出入,例如探测时间设定为5秒,txt文件内包含的数据条目为497092条,lvx文件内有495100条。注释掉保存txt的算法后,lvx文件内有495000条数据。抽样调查发现lvx文件内的部分数据在txt文件内找不到,而且txt文件内有部分数据的位置和lvx文件相比发生了偏移。
再次更新,发现lvx文件内的数据在txt文件里找不到很有可能是因为用记事本查找的时候只能单向查找。。发现了这个细节之后目前没发现lvx里有而txt里没有的条目。
几篇介绍thread并行的文章:
https://www.cnblogs.com/yssjun/p/11302500.html
https://www.cnblogs.com/yssjun/p/11533346.html
把昨天写的WriteFile方法优化一下,文件命名格式参照lvx文件命名格式,用时间作为文件名:
//二维数组点云坐标信息写入txt文件
void WriteFile(int PointData[100][4])
{
time_t curtime = time(nullptr);//根据当前系统时间获取文件名
char FileName[60] = { 0 };
tm* local_time = localtime(&curtime);
strftime(FileName, sizeof(FileName), "E:\\livox\\txtFiles\\%Y-%m-%d_%H-%M-%S.txt", local_time);
FILE* fp = fopen(FileName, "a");//文件指针,add方式
if (fp == NULL)
{
cout << "Failed to open file." << endl;
return;
}//错误处理
for (int i = 0; i < 100; i++)
{
for (int j = 0; j < 4; j++)
{
fprintf(fp, "%d", PointData[i][j]);
fputc(',', fp);
}
fprintf(fp, "\n");
}
fclose(fp);
}
但是这样会带来一个问题,因为这个函数是在每秒的每一帧里的每一个数据包保存时都要循环一次的,如果本次探测的时长大于1秒,那么由于按照当前精确到秒的命名规则,本次探测就会生成不止一个txt文件,而正常的逻辑应该是每次探测只生成一个文件。所以可能还是要把文件命名方法和文件保存方法分开来。
文件命名方法:
void GetFileName(char* FileName)
{
time_t curtime = time(nullptr);//根据当前系统时间获取文件名
char theFileName[60] = {0};
tm* local_time = localtime(&curtime);
strftime(theFileName, sizeof(theFileName), "E:\\livox\\txtFiles\\%Y-%m-%d_%H-%M-%S.txt", local_time);
strcpy(FileName,theFileName);
}
文件保存方法:
void WriteFile(int PointData[100][4],const char *FileName)
{
//time_t curtime = time(nullptr);//根据当前系统时间获取文件名
//char FileName[60] = {0};
//tm* local_time = localtime(&curtime);
//strftime(FileName, sizeof(FileName), "E:\\livox\\txtFiles\\%Y-%m-%d_%H-%M-%S.txt", local_time);
char* theFileName = new char[60];
strcpy(theFileName, FileName);
FILE* fp = fopen(theFileName,"a");//文件指针,add方式
if (fp == NULL)
{
cout << "Failed to open file." << endl;
return;
}//错误处理
for (int i = 0; i < 100; i++)
{
for (int j = 0; j < 4; j++)
{
fprintf(fp,"%d",PointData[i][j]);
fputc(',',fp);
}
fprintf(fp, "\n");
}
fclose(fp);
delete[] theFileName;
}
调用:
int Data[100][4];
for (int i = 0; i < 100; i++)
{
for (int j = 0; j<4; j++)
Data[i][j] = i * 4 + j;
}
char* FileName = new char[60];
GetFileName(FileName);
WriteFile(Data, FileName);
delete[] FileName;
下一步研究在程序运行后多次获取雷达数据的方法。