学习日志2020.7.14 解析Livox点云存储数据结构
点云数据存储方式终于有点眉目了。
点云数据率是每秒十万点，每秒有20帧frame rate，每一帧包含50个左右数据包point packet，这50个数据包组合成一个临时数据包列表point packet list temp，所有的临时数据包列表组成完整的数据包列表point packet list，写入本地文件。
算法层上，数据存储的最小单元是数据包point packet，每个数据包大小为1319字节byte，包含100个点的信息，这些信息包含100个点公用的头文件信息，和每个点单独的data信息，包括xyz和反射率。
为了研究point packet内部的数据存储结构，解析出来一个packet的数据和输出文件数据如下：
| 
判断没错的话这两张表内的数据应该是对应的，共包含了100个点的信息。
根据表格中后三个点的各项都是0，判断packet内包含数据的部分的最后39个byte应该都是0，这点可以对应上。
point_packet数据包里的每13个uint8_t类型的数据表示一个点的xyz坐标和反射率信息，1-4，5-8,9-12组合为三个uint32_t表示xyz，第13个数据表示反射率（0-255）。接下来把用uint8_t数据格式的坐标表示为int格式的坐标，单位为毫米。代码：
//十进制-二进制
int DecToBin(uint8_t Dec1/*,uint8_t Dec2,uint8_t Dec3,uint8_t Dec4*/)
{
int result = 0, temp = Dec1, j = 1;
while (temp) {
result = result + j * (temp % 2);
temp = temp / 2;
j = j * 10;
}
return result;
}
//4个uint8_t转换为int
int Dec4ToBin(uint8_t Dec1,uint8_t Dec2,uint8_t Dec3,uint8_t Dec4)
{
int result1 = DecToBin(Dec1);
int result2 = DecToBin(Dec2);
int result3 = DecToBin(Dec3);
int result4 = DecToBin(Dec4);
/*char *temp1 = (char *)malloc(8);
itoa(result1, temp1, 10);
char *temp2 = (char *)malloc(8);
itoa(result2, temp2, 10);
char *temp3 = (char *)malloc(8);
itoa(result3, temp3, 10);
char *temp4 = (char *)malloc(8);
itoa(result4, temp4, 10);*/
char *resultChar = (char *)malloc(32);
//sprintf(resultChar, "%s%s%s%s", temp4, temp3, temp2,temp1);
sprintf(resultChar,"%.8d%.8d%.8d%.8d",result4,result3,result2,result1);//补位到8
//cout << resultChar << endl;
int i=0,n=0,m=1,result=0;
//for (result = i = 0; resultChar[i]; (result *= 2) += resultChar[i++] - '0');
while (resultChar[i++])
n++; //统计实际有多少位
for (i = 0; i<n; i++)
{
result = result + (resultChar[n - i - 1] - '0')*m;
//由于输入是字符，所以减掉'0'得到对应的数值，另外采用低位先加
m *= 2; //因为高位的权是低位的2倍，所以每加完一位，权乘2
}
return result;
}
用这种方法对雷达点云数据进行解析，在控制台获取计算后的点坐标，和sdk生成的本地lvx文件进行对比，能够吻合。
控制台信息：
Livox SDK initializing.
Livox SDK has been initialized.
Livox SDK version 2.1.1 .
Start discovering device.
[2020-07-14 16:34:57.344] [console] [info] Broadcast broadcast code: 0TFDH35006
Q61X1 [device_discovery.cpp] [OnBroadcast] [162]
Receive Broadcast Code 0TFDH35006Q61X1
[2020-07-14 16:34:57.345] [console] [info] Broadcast code : 0TFDH35006Q61X1 not
add to connect [device_discovery.cpp] [OnBroadcast] [179]
[2020-07-14 16:34:58.352] [console] [info] Broadcast broadcast code: 0TFDH35006
Q61X1 [device_discovery.cpp] [OnBroadcast] [162]
Receive Broadcast Code 0TFDH35006Q61X1
[2020-07-14 16:34:58.353] [console] [info] Broadcast code : 0TFDH35006Q61X1 not
add to connect [device_discovery.cpp] [OnBroadcast] [179]
[2020-07-14 16:34:59.360] [console] [info] Broadcast broadcast code: 0TFDH35006
Q61X1 [device_discovery.cpp] [OnBroadcast] [162]
Receive Broadcast Code 0TFDH35006Q61X1
[2020-07-14 16:34:59.366] [console] [info] LocalIP: 192.168.1.5 [device_discove
ry.cpp] [OnBroadcast] [225]
[2020-07-14 16:34:59.366] [console] [info] Command Port: 55501 [device_discover
y.cpp] [OnBroadcast] [226]
[2020-07-14 16:34:59.366] [console] [info] Data Port: 56001 [device_discovery.c
pp] [OnBroadcast] [227]
[2020-07-14 16:34:59.384] [console] [info] New Device [device_discovery.cpp] [O
nData] [109]
[2020-07-14 16:34:59.385] [console] [info] Handle: 0 [device_discovery.cpp] [On
Data] [110]
[2020-07-14 16:34:59.385] [console] [info] Broadcast Code: 0TFDH35006Q61X1 [dev
ice_discovery.cpp] [OnData] [111]
[2020-07-14 16:34:59.385] [console] [info] Type: 1 [device_discovery.cpp] [OnDa
ta] [112]
[2020-07-14 16:34:59.386] [console] [info] IP: 192.168.1.78 [device_discovery.c
pp] [OnData] [113]
[2020-07-14 16:34:59.386] [console] [info] Command Port: 55501 [device_discover
y.cpp] [OnData] [114]
[2020-07-14 16:34:59.386] [console] [info] Data Port: 56001 [device_discovery.c
pp] [OnData] [115]
[2020-07-14 16:34:59.389] [console] [info] Send Command: Set 0 Id 2 Seq 3 [com
mand_channel.cpp] [Send] [241]
[2020-07-14 16:34:59.408] [console] [info] Recieve Ack: Set 0 Id 2 Seq 3 [comm
and_handler.cpp] [OnCommand] [129]
OnDeviceChange broadcast code 0TFDH35006Q61X1 update type 0
[WARNING] Lidar sn: [0TFDH35006Q61X1] Connect!!!
Device Working State 5
Device State Error Code 0X00000000
Device feature 0
[2020-07-14 16:34:59.410] [console] [info] Send Command: Set 0 Id 2 Seq 4 [com
mand_channel.cpp] [Send] [241]
[2020-07-14 16:34:59.432] [console] [info] Update State to 1, device connect tr
ue [device_manager.cpp] [UpdateDeviceState] [265]
OnDeviceChange broadcast code 0TFDH35006Q61X1 update type 2
[WARNING] Lidar sn: [0TFDH35006Q61X1] StateChange!!!
Device Working State 1
Device State Error Code 0X00000800
Device feature 0
[2020-07-14 16:34:59.434] [console] [info] Recieve Ack: Set 0 Id 2 Seq 4 [comm
and_handler.cpp] [OnCommand] [129]
firm ver: 3.7.0.0
[2020-07-14 16:34:59.434] [console] [info] Send Command: Set 1 Id 2 Seq 6 [com
mand_channel.cpp] [Send] [241]
[2020-07-14 16:34:59.435] [console] [info] Send Command: Set 0 Id 4 Seq 7 [com
mand_channel.cpp] [Send] [241]
[2020-07-14 16:34:59.456] [console] [info] Recieve Ack: Set 1 Id 2 Seq 6 [comm
and_handler.cpp] [OnCommand] [129]
OnGetLidarExtrinsicParameter statue 0 handle 0 response 0
Start initialize lvx file.
[2020-07-14 16:34:59.458] [console] [info] Recieve Ack: Set 0 Id 4 Seq 7 [comm
and_handler.cpp] [OnCommand] [129]
OnSampleCallback statues 0 handle 0 response 0
point packet list temp size is:53
Packet 0:
4070 -572 -191 70
0 0 0 0
0 0 0 0
0 0 0 0
2214 -328 -107 15
2211 -332 -108 16
2210 -336 -109 16
2207 -339 -109 16
2206 -344 -110 16
2205 -348 -111 16
2200 -351 -111 17
2199 -355 -112 17
2203 -360 -113 16
2191 -362 -113 15
2196 -367 -114 15
2195 -371 -115 15
2191 -374 -115 14
2189 -378 -116 14
2184 -381 -117 15
2188 -386 -117 16
2187 -390 -118 16
2185 -393 -118 16
2190 -398 -119 18
2190 -402 -120 18
2187 -406 -120 18
2189 -410 -121 19
2188 -414 -122 19
2183 -417 -122 19
2182 -421 -122 19
2180 -424 -123 19
2184 -429 -123 19
2184 -433 -124 20
2179 -436 -124 20
2179 -439 -124 20
2176 -443 -125 19
2174 -446 -125 22
2173 -450 -125 22
2167 -452 -125 21
2168 -456 -126 22
2166 -460 -126 21
2168 -464 -126 22
2160 -466 -126 22
2161 -470 -127 21
2161 -473 -127 23
2163 -477 -127 23
2157 -480 -127 22
2156 -483 -127 22
2154 -487 -128 22
2154 -490 -128 20
2151 -493 -128 21
2154 -497 -128 21
2147 -499 -128 23
2149 -503 -128 21
2146 -506 -128 23
2147 -510 -128 21
2145 -513 -128 23
2143 -516 -128 23
2136 -517 -128 22
2142 -522 -128 23
2139 -525 -128 24
2133 -527 -128 22
2132 -530 -128 22
2136 -534 -128 24
2127 -535 -127 23
2127 -538 -127 23
2126 -541 -127 23
2125 -544 -127 23
2124 -547 -127 23
2130 -552 -127 25
2123 -553 -127 23
2121 -556 -126 23
2127 -561 -127 25
2120 -562 -126 23
2125 -567 -126 25
2118 -568 -126 23
2113 -570 -125 24
2113 -572 -125 22
2097 -571 -124 1
2104 -576 -124 1
2115 -582 -124 1
2120 -586 -124 1
0 0 0 0
0 0 0 0
1480 -415 -86 1
1204 -340 -70 1
1188 -336 -69 1
1187 -338 -68 1
1178 -337 -68 1
1187 -341 -68 1
1177 -340 -67 1
1169 -339 -67 1
1177 -342 -67 1
1185 -346 -67 1
1185 -348 -67 1
1181 -348 -66 1
1167 -345 -65 1
1187 -353 -66 1
1187 -354 -66 1
1183 -354 -65 1
1182 -356 -65 1
Finish save 0 frame to lvx file.
point packet list temp size is:9
Packet 0:
3952 -849 760 109
3954 -844 758 109
3955 -840 756 109
3954 -835 754 107
3956 -831 752 107
3960 -827 750 107
3960 -822 748 109
3962 -817 746 109
3969 -814 745 102
3968 -809 742 104
3973 -805 741 106
3972 -800 738 102
3973 -795 735 102
3979 -791 734 97
3977 -786 731 93
3983 -782 729 90
3983 -777 726 94
3985 -773 724 95
3986 -768 721 97
3992 -764 719 102
3994 -759 717 102
3990 -753 713 109
3997 -749 711 102
3998 -744 708 102
4001 -740 706 105
4003 -735 703 106
4007 -730 700 108
4009 -725 697 110
4012 -721 694 108
4012 -715 691 106
4019 -711 689 103
4019 -706 685 105
4023 -701 682 108
4024 -696 679 103
4028 -691 676 110
4029 -686 673 110
4032 -681 669 109
4032 -676 666 107
4037 -671 663 107
4043 -667 660 106
4048 -663 657 104
4050 -657 653 113
4048 -652 649 108
4054 -647 646 106
4057 -642 643 104
4061 -637 639 113
4061 -631 635 113
4065 -627 631 109
4070 -622 628 110
4074 -617 624 114
4076 -612 621 112
4080 -607 617 114
4081 -601 613 114
4088 -597 609 110
4096 -592 606 98
4097 -587 602 98
4106 -582 598 104
4101 -576 593 100
4109 -571 590 104
4111 -566 585 104
4112 -560 581 104
4117 -555 577 109
4119 -550 572 109
4119 -544 567 111
4118 -539 562 115
4127 -534 559 106
4128 -528 554 106
4132 -523 550 106
4131 -517 544 106
4135 -512 540 106
4141 -507 535 113
4137 -501 530 108
4144 -496 525 108
4148 -490 521 102
4150 -485 516 102
4156 -480 511 109
4155 -474 506 109
4150 -468 500 113
4165 -464 497 109
4163 -457 491 109
4165 -452 485 109
4167 -446 480 109
4170 -441 475 109
4172 -435 470 109
4173 -429 464 109
4175 -424 459 116
4178 -418 453 110
4179 -413 448 116
4180 -407 442 116
4182 -401 437 116
4184 -396 431 117
4188 -390 426 115
4192 -385 420 108
4194 -379 414 117
4194 -373 408 115
4196 -367 403 117
4201 -362 397 112
4205 -357 391 115
4213 -351 386 116
4206 -345 379 110
Finish save 1 frame to lvx file.
[2020-07-14 16:34:59.739] [console] [info] Send Command: Set 0 Id 4 Seq 8 [com
mand_channel.cpp] [Send] [241]
本地lvx文件转csv文件：
点云数据包解析和本地保存应该没问题了，接下来才会遇到最有难度的问题，每秒内的点云数据有20帧每帧50包，这么大的数据量如何避免阻塞呢。还有一个问题，在做上面这个实验时，把雷达运行时间限定在了0.1秒内的2帧，在控制台上可以看到，这两帧内的至少第一个数据包都成功获取到了数据，但是在sdk生成的本地文件里只能看到第一帧获取的数据。如果把雷达运行时间限定为1帧，那么sdk生成的本地文件就会为空，尽管控制台上可以看到确实获取到了数据。
顺便写一个把数据包写入本地txt文件的算法：
void WriteFile(int PointData[100][4])
{
FILE* fp = fopen("Livox1.txt","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);
}
参数为100行4列的二维数组，保存的是一个point packet内100个点的xyz坐标和反射率，以add方式写入txt文件。把一个数据包内的坐标信息保存到本地的操作执行1000次大约用时0.5秒之内，livox雷达一秒内获取的数据包在1000个上下。