hayodea/salmanoff
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 2 Wiki Activity

livoxG1: Add point cloud frame collator OpenCL kernel

Browse Source
This commit is contained in:
Hayodea Hekol
2025-11-09 04:48:15 -04:00
parent 4b60a10bc6
commit 6264a128a8
1 changed files with 210 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
stimBuffApis/livoxGen1/collateDgrams.cl
+210 -2
View File
@@ -1,3 +1,211 @@
__kernel void collate(__global uchar* assembly, __global uchar* collation) {
// Placeholder kernel - will be replaced with actual collation logic.
__kernel void collate(
__global uchar* assembly,
__global float* collation,
uint slotStride,
uint firstSlotOffset,
uint nPointsPerSlot,
uint nDgramsPerFrame)
{
// Get work item index (slot index)
uint slotIndex = get_global_id(0);
// Bounds check
if (slotIndex >= nDgramsPerFrame)
{
return;
}
// Calculate slot address
__global uchar* slotStart = assembly + firstSlotOffset
+ (slotIndex * slotStride);
// Read data_type from offset 9 (1 byte)
uchar dataType = slotStart[9];
// Get points array pointer (after 18-byte header)
__global uchar* pointsArray = slotStart + 18;
// Base offset in collation buffer for this slot (in floats, 4 per PointXYZI)
uint collationBaseOffset = slotIndex * nPointsPerSlot * 4;
// Process based on data type using nested ifs (outer) with loops (inner)
if (dataType == 0)
{
// Type 0: LivoxRawPoint - 13 bytes per point
// Structure: int32_t x, y, z (mm), uint8_t reflectivity
for (uint i = 0; i < nPointsPerSlot; ++i)
{
__global uchar* pointPtr = pointsArray + (i * 13);
// Read int coordinates (little-endian)
int x_mm = *((__global int*)(pointPtr + 0));
int y_mm = *((__global int*)(pointPtr + 4));
int z_mm = *((__global int*)(pointPtr + 8));
uchar reflectivity = pointPtr[12];
// Convert to PointXYZI (meters, float)
float x = (float)x_mm / 1000.0f;
float y = (float)y_mm / 1000.0f;
float z = (float)z_mm / 1000.0f;
float intensity = (float)reflectivity;
// Write to collation buffer
uint offset = collationBaseOffset + (i * 4);
collation[offset + 0] = x;
collation[offset + 1] = y;
collation[offset + 2] = z;
collation[offset + 3] = intensity;
}
}
else if (dataType == 2)
{
// Type 2: LivoxExtendRawPoint - 14 bytes per point
// Structure: int32_t x, y, z (mm), uint8_t reflectivity, uint8_t tag (ignored)
for (uint i = 0; i < nPointsPerSlot; ++i)
{
__global uchar* pointPtr = pointsArray + (i * 14);
// Read int coordinates (little-endian)
int x_mm = *((__global int*)(pointPtr + 0));
int y_mm = *((__global int*)(pointPtr + 4));
int z_mm = *((__global int*)(pointPtr + 8));
uchar reflectivity = pointPtr[12];
// tag at offset 13 is ignored
// Convert to PointXYZI (meters, float)
float x = (float)x_mm / 1000.0f;
float y = (float)y_mm / 1000.0f;
float z = (float)z_mm / 1000.0f;
float intensity = (float)reflectivity;
// Write to collation buffer
uint offset = collationBaseOffset + (i * 4);
collation[offset + 0] = x;
collation[offset + 1] = y;
collation[offset + 2] = z;
collation[offset + 3] = intensity;
}
}
else if (dataType == 4)
{
// Type 4: LivoxDualExtendRawPoint - 28 bytes per sample (2 points)
// Structure: point1 (x1,y1,z1,reflectivity1,tag1), point2 (x2,y2,z2,reflectivity2,tag2)
// nPointsPerSlot should be 96, but we have 48 samples * 2 points = 96 points
uint nSamples = nPointsPerSlot / 2;
uint pointIndex = 0;
for (uint i = 0; i < nSamples; ++i)
{
__global uchar* samplePtr = pointsArray + (i * 28);
// Process first point
int x1_mm = *((__global int*)(samplePtr + 0));
int y1_mm = *((__global int*)(samplePtr + 4));
int z1_mm = *((__global int*)(samplePtr + 8));
uchar reflectivity1 = samplePtr[12];
// tag1 at offset 13 is ignored
float x1 = (float)x1_mm / 1000.0f;
float y1 = (float)y1_mm / 1000.0f;
float z1 = (float)z1_mm / 1000.0f;
float intensity1 = (float)reflectivity1;
uint offset1 = collationBaseOffset + (pointIndex * 4);
collation[offset1 + 0] = x1;
collation[offset1 + 1] = y1;
collation[offset1 + 2] = z1;
collation[offset1 + 3] = intensity1;
++pointIndex;
// Process second point
int x2_mm = *((__global int*)(samplePtr + 14));
int y2_mm = *((__global int*)(samplePtr + 18));
int z2_mm = *((__global int*)(samplePtr + 22));
uchar reflectivity2 = samplePtr[26];
// tag2 at offset 27 is ignored
float x2 = (float)x2_mm / 1000.0f;
float y2 = (float)y2_mm / 1000.0f;
float z2 = (float)z2_mm / 1000.0f;
float intensity2 = (float)reflectivity2;
uint offset2 = collationBaseOffset + (pointIndex * 4);
collation[offset2 + 0] = x2;
collation[offset2 + 1] = y2;
collation[offset2 + 2] = z2;
collation[offset2 + 3] = intensity2;
++pointIndex;
}
}
else if (dataType == 7)
{
// Type 7: LivoxTripleExtendRawPoint - 42 bytes per sample (3 points)
// Structure: point1, point2, point3 (each: x,y,z,reflectivity,tag)
// nPointsPerSlot should be 90, but we have 30 samples * 3 points = 90 points
uint nSamples = nPointsPerSlot / 3;
uint pointIndex = 0;
for (uint i = 0; i < nSamples; ++i)
{
__global uchar* samplePtr = pointsArray + (i * 42);
// Process first point
int x1_mm = *((__global int*)(samplePtr + 0));
int y1_mm = *((__global int*)(samplePtr + 4));
int z1_mm = *((__global int*)(samplePtr + 8));
uchar reflectivity1 = samplePtr[12];
// tag1 at offset 13 is ignored
float x1 = (float)x1_mm / 1000.0f;
float y1 = (float)y1_mm / 1000.0f;
float z1 = (float)z1_mm / 1000.0f;
float intensity1 = (float)reflectivity1;
uint offset1 = collationBaseOffset + (pointIndex * 4);
collation[offset1 + 0] = x1;
collation[offset1 + 1] = y1;
collation[offset1 + 2] = z1;
collation[offset1 + 3] = intensity1;
++pointIndex;
// Process second point
int x2_mm = *((__global int*)(samplePtr + 14));
int y2_mm = *((__global int*)(samplePtr + 18));
int z2_mm = *((__global int*)(samplePtr + 22));
uchar reflectivity2 = samplePtr[26];
// tag2 at offset 27 is ignored
float x2 = (float)x2_mm / 1000.0f;
float y2 = (float)y2_mm / 1000.0f;
float z2 = (float)z2_mm / 1000.0f;
float intensity2 = (float)reflectivity2;
uint offset2 = collationBaseOffset + (pointIndex * 4);
collation[offset2 + 0] = x2;
collation[offset2 + 1] = y2;
collation[offset2 + 2] = z2;
collation[offset2 + 3] = intensity2;
++pointIndex;
// Process third point
int x3_mm = *((__global int*)(samplePtr + 28));
int y3_mm = *((__global int*)(samplePtr + 32));
int z3_mm = *((__global int*)(samplePtr + 36));
uchar reflectivity3 = samplePtr[40];
// tag3 at offset 41 is ignored
float x3 = (float)x3_mm / 1000.0f;
float y3 = (float)y3_mm / 1000.0f;
float z3 = (float)z3_mm / 1000.0f;
float intensity3 = (float)reflectivity3;
uint offset3 = collationBaseOffset + (pointIndex * 4);
collation[offset3 + 0] = x3;
collation[offset3 + 1] = y3;
collation[offset3 + 2] = z3;
collation[offset3 + 3] = intensity3;
++pointIndex;
}
}
// Unsupported data types are silently ignored
}