feat: 改写GIGE协议

src/core/PointCloudProcessor.cpp

@@ -1,6 +1,11 @@
 #include "core/PointCloudProcessor.h"
 #include <QDebug>
 #include <vector>
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
 
 PointCloudProcessor::PointCloudProcessor(QObject *parent)
     : QObject(parent)
@@ -246,6 +251,72 @@ void PointCloudProcessor::processDepthData(const QByteArray &depthData, uint32_t
     emit pointCloudReady(cloud, blockId);
 }
 
+void PointCloudProcessor::processPointCloudData(const QByteArray &cloudData, uint32_t blockId)
+{
+    // qDebug() << "[PointCloud] Processing pre-computed point cloud data";
+    // qDebug() << "[PointCloud] Data size:" << cloudData.size() << "bytes";
+
+    // 验证数据大小：支持两种格式
+    // 格式1：只有Z坐标 (width * height * sizeof(int16_t))
+    // 格式2：完整XYZ (width * height * 3 * sizeof(int16_t))
+    size_t expectedSizeZ = m_imageWidth * m_imageHeight * sizeof(int16_t);
+    size_t expectedSizeXYZ = m_imageWidth * m_imageHeight * 3 * sizeof(int16_t);
+
+    bool isZOnly = (cloudData.size() == expectedSizeZ);
+    bool isXYZ = (cloudData.size() == expectedSizeXYZ);
+
+    if (!isZOnly && !isXYZ) {
+        qDebug() << "[PointCloud] ERROR: Invalid point cloud data size:" << cloudData.size()
+                 << "expected:" << expectedSizeZ << "(Z only) or" << expectedSizeXYZ << "(XYZ)";
+        return;
+    }
+
+    // qDebug() << "[PointCloud] Data format:" << (isZOnly ? "Z only" : "XYZ");
+
+    // 创建PCL点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    cloud->width = m_imageWidth;
+    cloud->height = m_imageHeight;
+    cloud->is_dense = false;
+    cloud->points.resize(m_totalPoints);
+
+    // 从int16_t数组读取点云数据
+    const int16_t* cloudShort = reinterpret_cast<const int16_t*>(cloudData.constData());
+
+    if (isZOnly) {
+        // Z-only格式：直接映射为平面点云（类似图像平面）
+        // X对应列（左右翻转），Y对应行（上下翻转），Z是深度值
+        // qDebug() << "[PointCloud] Processing Z-only format as planar mapping";
+
+        for (size_t i = 0; i < m_totalPoints; i++) {
+            int row = i / m_imageWidth;
+            int col = i % m_imageWidth;
+
+            // 直接映射：X=列（翻转），Y=行（翻转），Z=深度
+            float x = static_cast<float>(m_imageWidth - 1 - col);  // 左右翻转
+            float y = static_cast<float>(m_imageHeight - 1 - row);  // 上下翻转
+            float z = static_cast<float>(cloudShort[i]) * m_zScale;
+
+            cloud->points[i].x = x;
+            cloud->points[i].y = y;
+            cloud->points[i].z = z;
+        }
+    } else {
+        // XYZ格式：完整的三维坐标
+    // qDebug() << "[PointCloud] Processing XYZ format";
+
+        for (size_t i = 0; i < m_totalPoints; i++) {
+            cloud->points[i].x = static_cast<float>(cloudShort[i * 3 + 0]) * m_zScale;
+            cloud->points[i].y = static_cast<float>(cloudShort[i * 3 + 1]) * m_zScale;
+            cloud->points[i].z = static_cast<float>(cloudShort[i * 3 + 2]) * m_zScale;
+        }
+    }
+
+    // qDebug() << "[PointCloud] Block" << blockId << "processed successfully,"
+    // << m_totalPoints << "points";
+    emit pointCloudReady(cloud, blockId);
+}
+
 void PointCloudProcessor::cleanupOpenCL()
 {
     if (m_depthBuffer) {