212 lines
6.5 KiB
Common Lisp
212 lines
6.5 KiB
Common Lisp
__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
|
|
}
|