diff --git a/stimBuffApis/livoxGen1/collateDgrams.cl b/stimBuffApis/livoxGen1/collateDgrams.cl index 285db78..ae85b2f 100644 --- a/stimBuffApis/livoxGen1/collateDgrams.cl +++ b/stimBuffApis/livoxGen1/collateDgrams.cl @@ -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 }