diff --git a/.gitignore b/.gitignore index e1796d0..93fe094 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,5 @@ build/** */.venv -CLAUDE.md \ No newline at end of file +CLAUDE.md +.claude/ \ No newline at end of file diff --git a/include/core/GVSPParser.h b/include/core/GVSPParser.h index 7a479b1..192410c 100644 --- a/include/core/GVSPParser.h +++ b/include/core/GVSPParser.h @@ -4,16 +4,18 @@ #include #include #include +#include #include -// GVSP packet types +// GVSP packet types (GigE Vision 2.1 standard) #define GVSP_LEADER_PACKET 0x01 -#define GVSP_PAYLOAD_PACKET 0x02 -#define GVSP_TRAILER_PACKET 0x03 +#define GVSP_TRAILER_PACKET 0x02 +#define GVSP_PAYLOAD_PACKET 0x03 // Payload types -#define PAYLOAD_TYPE_IMAGE 0x0001 -#define PAYLOAD_TYPE_BINARY 0x0003 +#define PAYLOAD_TYPE_IMAGE 0x0001 +#define PAYLOAD_TYPE_BINARY 0x0003 +#define PAYLOAD_TYPE_POINTCLOUD 0x8000 // Vendor-specific for point cloud data // Image format #define PIXEL_FORMAT_12BIT_GRAY 0x010C0001 @@ -24,11 +26,13 @@ #pragma pack(push, 1) -// GVSP packet header +// GVSP packet header (GigE Vision 2.1 standard - 8 bytes) struct GVSPPacketHeader { - uint16_t status; - uint16_t block_id; - uint32_t packet_fmt_id; + uint16_t status; // Status flags + uint16_t block_id; // Block ID (frame number) + uint8_t packet_format; // Packet type: 0x01=Leader, 0x02=Trailer, 0x03=Payload + uint8_t reserved; // Reserved byte + uint16_t packet_id; // Packet ID within block }; // Image data leader @@ -71,6 +75,22 @@ struct GVSPBinaryDataTrailer { uint32_t checksum; }; +// Point cloud data leader (vendor-specific, payload_type = 0x8000) +struct GVSPPointCloudDataLeader { + uint16_t reserved; + uint16_t payload_type; // 0x8000 + uint32_t timestamp_high; + uint32_t timestamp_low; + uint32_t data_size; // Total size of point cloud data +}; + +// Point cloud data trailer +struct GVSPPointCloudDataTrailer { + uint32_t reserved; + uint16_t payload_type; // 0x8000 + uint32_t checksum; +}; + #pragma pack(pop) class GVSPParser : public QObject @@ -87,6 +107,7 @@ public: signals: void imageReceived(const QImage &image, uint32_t blockId); void depthDataReceived(const QByteArray &depthData, uint32_t blockId); + void pointCloudDataReceived(const QByteArray &cloudData, uint32_t blockId); void parseError(const QString &error); private: @@ -96,11 +117,12 @@ private: void processImageData(); void processDepthData(); + void processPointCloudData(); private: // Reception state bool m_isReceiving; - int m_dataType; // 0=unknown, 1=image, 3=depth + int m_dataType; // 0=unknown, 1=image, 3=depth, 4=pointcloud uint32_t m_currentBlockId; // Data buffer @@ -116,6 +138,9 @@ private: // Statistics uint32_t m_lastBlockId; int m_packetCount; + + // Async processing control + QAtomicInt m_imageProcessingCount; }; #endif // GVSPPARSER_H diff --git a/include/core/PointCloudProcessor.h b/include/core/PointCloudProcessor.h index e12eca8..22d715e 100644 --- a/include/core/PointCloudProcessor.h +++ b/include/core/PointCloudProcessor.h @@ -27,6 +27,9 @@ public: // 将深度数据转换为点云（使用OpenCL GPU加速） void processDepthData(const QByteArray &depthData, uint32_t blockId); + // 处理已经计算好的点云数据（x,y,z格式） + void processPointCloudData(const QByteArray &cloudData, uint32_t blockId); + signals: void pointCloudReady(pcl::PointCloud::Ptr cloud, uint32_t blockId); void errorOccurred(const QString &error); diff --git a/src/core/GVSPParser.cpp b/src/core/GVSPParser.cpp index 90a2796..d49b6df 100644 --- a/src/core/GVSPParser.cpp +++ b/src/core/GVSPParser.cpp @@ -1,5 +1,6 @@ #include "core/GVSPParser.h" #include +#include #include #include @@ -16,6 +17,7 @@ GVSPParser::GVSPParser(QObject *parent) , m_lastBlockId(0) , m_packetCount(0) { + m_imageProcessingCount.storeRelaxed(0); } GVSPParser::~GVSPParser() @@ -35,6 +37,8 @@ void GVSPParser::reset() void GVSPParser::parsePacket(const QByteArray &packet) { + static int debugCount = 0; + if (packet.size() < sizeof(GVSPPacketHeader)) { return; } @@ -42,27 +46,20 @@ void GVSPParser::parsePacket(const QByteArray &packet) const uint8_t *data = reinterpret_cast(packet.constData()); const GVSPPacketHeader *header = reinterpret_cast(data); - // 注释掉调试日志以提高性能 - // static int debugCount = 0; - // if (debugCount < 3) { - // qDebug() << "Packet" << debugCount << "first 16 bytes (hex):"; - // QString hexStr; - // for (int i = 0; i < qMin(16, packet.size()); i++) { - // hexStr += QString("%1 ").arg(data[i], 2, 16, QChar('0')); - // } - // qDebug() << hexStr; - // debugCount++; - // } - - // GVSP包头格式：status(2) + block_id(2) + packet_format(4) - // 包类型在packet_format的高字节 - uint32_t packet_fmt = ntohl(header->packet_fmt_id); - uint8_t packetType = (packet_fmt >> 24) & 0xFF; + // GigE Vision 2.1 标准 GVSP 包头解析 uint16_t blockId = ntohs(header->block_id); + uint8_t packetType = header->packet_format; + uint16_t packetId = ntohs(header->packet_id); - // 注释掉频繁的日志输出以提高性能 - // static int leaderCount = 0; - // static int trailerCount = 0; + // 打印前5个包的详细信息 + if (debugCount < 5) { + // qDebug() << "[GVSPParser] Packet" << debugCount + // << "Type:" << packetType + // << "BlockID:" << blockId + // << "PacketID:" << packetId + // << "Size:" << packet.size(); + debugCount++; + } switch (packetType) { case GVSP_LEADER_PACKET: @@ -81,7 +78,9 @@ void GVSPParser::parsePacket(const QByteArray &packet) void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size) { - if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) { + // 最小大小检查：至少要有包头 + 2字节reserved + 2字节payload_type + if (size < sizeof(GVSPPacketHeader) + 4) { + qDebug() << "[GVSPParser] Leader packet too small:" << size << "bytes"; return; } @@ -92,8 +91,16 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size) const uint16_t *payload_type_ptr = reinterpret_cast(data + sizeof(GVSPPacketHeader) + 2); uint16_t payload_type = ntohs(*payload_type_ptr); +// qDebug() << "[GVSPParser] Leader packet: BlockID=" << m_currentBlockId +// << "PayloadType=0x" << Qt::hex << payload_type << Qt::dec +// << "Size=" << size; + if (payload_type == PAYLOAD_TYPE_IMAGE) { // Image data leader + if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) { + qDebug() << "[GVSPParser] Image leader too small"; + return; + } const GVSPImageDataLeader *leader = reinterpret_cast(data + sizeof(GVSPPacketHeader)); m_dataType = 1; @@ -115,10 +122,10 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size) else if (payload_type == PAYLOAD_TYPE_BINARY) { // Depth data leader const GVSPBinaryDataLeader *leader = reinterpret_cast(data + sizeof(GVSPPacketHeader)); - + m_dataType = 3; m_expectedSize = ntohl(leader->file_size); - + m_dataBuffer.clear(); m_dataBuffer.reserve(m_expectedSize); m_receivedSize = 0; @@ -129,6 +136,22 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size) // qDebug() << "Depth Leader: Block" << m_currentBlockId // << "Size:" << m_expectedSize << "bytes"; } + else if (payload_type == PAYLOAD_TYPE_POINTCLOUD) { + // Point cloud data leader (vendor-specific 0x8000) + const GVSPPointCloudDataLeader *leader = reinterpret_cast(data + sizeof(GVSPPacketHeader)); + + m_dataType = 4; // 新类型：点云数据 + m_expectedSize = ntohl(leader->data_size); + + m_dataBuffer.clear(); + m_dataBuffer.reserve(m_expectedSize); + m_receivedSize = 0; + m_isReceiving = true; + m_packetCount = 0; + + // qDebug() << "[PointCloud Leader] Block:" << m_currentBlockId + // << "Expected Size:" << m_expectedSize << "bytes"; + } } void GVSPParser::handlePayloadPacket(const uint8_t *data, size_t size) @@ -167,6 +190,8 @@ void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size) processImageData(); } else if (m_dataType == 3) { processDepthData(); + } else if (m_dataType == 4) { + processPointCloudData(); } // Reset state @@ -177,43 +202,60 @@ void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size) void GVSPParser::processImageData() { if (m_dataBuffer.size() < m_expectedSize) { - // 注释掉频繁的警告日志 - // qDebug() << "Warning: Incomplete image data" << m_dataBuffer.size() << "/" << m_expectedSize; return; } - // Convert 16-bit depth data to 8-bit grayscale for display - const uint16_t *src = reinterpret_cast(m_dataBuffer.constData()); - QImage image(m_imageWidth, m_imageHeight, QImage::Format_Grayscale8); - - // Find min/max for normalization - uint16_t minVal = 65535, maxVal = 0; - for (size_t i = 0; i < m_imageWidth * m_imageHeight; i++) { - uint16_t val = src[i]; - if (val > 0) { - if (val < minVal) minVal = val; - if (val > maxVal) maxVal = val; - } + // 节流机制：如果已有3个或更多图像在处理中，跳过当前帧 + if (m_imageProcessingCount.loadAcquire() >= 3) { + return; } - // Normalize to 0-255 - uint8_t *dst = image.bits(); - float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f; + // 增加处理计数 + m_imageProcessingCount.ref(); - for (size_t y = 0; y < m_imageHeight; y++) { - for (size_t x = 0; x < m_imageWidth; x++) { - size_t idx = y * m_imageWidth + x; + // 复制数据到局部变量，避免在异步处理时数据被覆盖 + QByteArray dataCopy = m_dataBuffer; + uint32_t blockId = m_currentBlockId; + size_t width = m_imageWidth; + size_t height = m_imageHeight; - uint16_t val = src[idx]; - if (val == 0) { - dst[idx] = 0; - } else { - dst[idx] = static_cast((val - minVal) * scale); + // 使用QtConcurrent在后台线程处理图像数据 + QtConcurrent::run([this, dataCopy, blockId, width, height]() { + // Convert 16-bit depth data to 8-bit grayscale for display + const uint16_t *src = reinterpret_cast(dataCopy.constData()); + QImage image(width, height, QImage::Format_Grayscale8); + + // 优化：使用采样方式快速估算min/max（每隔16个像素采样一次） + uint16_t minVal = 65535, maxVal = 0; + size_t totalPixels = width * height; + const size_t sampleStep = 16; // 采样步长 + + for (size_t i = 0; i < totalPixels; i += sampleStep) { + uint16_t val = src[i]; + if (val > 0) { + if (val < minVal) minVal = val; + if (val > maxVal) maxVal = val; } } - } - emit imageReceived(image, m_currentBlockId); + // 归一化并水平翻转（修正左右镜像） + uint8_t *dst = image.bits(); + float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f; + + for (size_t row = 0; row < height; row++) { + for (size_t col = 0; col < width; col++) { + size_t srcIdx = row * width + col; + size_t dstIdx = row * width + (width - 1 - col); // 水平翻转 + uint16_t val = src[srcIdx]; + dst[dstIdx] = (val == 0) ? 0 : static_cast((val - minVal) * scale); + } + } + + emit imageReceived(image, blockId); + + // 减少处理计数 + m_imageProcessingCount.deref(); + }); } void GVSPParser::processDepthData() @@ -226,3 +268,18 @@ void GVSPParser::processDepthData() emit depthDataReceived(m_dataBuffer, m_currentBlockId); } + +void GVSPParser::processPointCloudData() +{ + // qDebug() << "[PointCloud] Received:" << m_dataBuffer.size() + // << "bytes, Expected:" << m_expectedSize << "bytes"; + + if (m_dataBuffer.size() < m_expectedSize) { + qDebug() << "[PointCloud] ERROR: Data incomplete, skipping!"; + return; + } + + // qDebug() << "[PointCloud] Data complete, emitting pointCloudDataReceived signal..."; + // 点云数据直接发送，格式为 short 数组 (x, y, z, x, y, z, ...) + emit pointCloudDataReceived(m_dataBuffer, m_currentBlockId); +} diff --git a/src/core/Logger.cpp b/src/core/Logger.cpp index a7cbfeb..c555e88 100644 --- a/src/core/Logger.cpp +++ b/src/core/Logger.cpp @@ -15,7 +15,7 @@ Logger* Logger::instance() Logger::Logger(QObject *parent) : QObject(parent) - , m_maxLines(10000) // 保留最新10000行 + , m_maxLines(100000) // 保留最新10000行 , m_currentLines(0) { } diff --git a/src/core/NetworkManager.cpp b/src/core/NetworkManager.cpp index fc9b7e4..fc53213 100644 --- a/src/core/NetworkManager.cpp +++ b/src/core/NetworkManager.cpp @@ -19,13 +19,14 @@ NetworkManager::NetworkManager(QObject *parent) // 连接GVSP解析器信号 connect(m_gvspParser, &GVSPParser::imageReceived, this, &NetworkManager::imageReceived); connect(m_gvspParser, &GVSPParser::depthDataReceived, this, &NetworkManager::depthDataReceived); + connect(m_gvspParser, &GVSPParser::pointCloudDataReceived, this, &NetworkManager::pointCloudDataReceived); } NetworkManager::~NetworkManager() { disconnectFromCamera(); } -#define if(x) if ((x) && (rand() < RAND_MAX * 0.50)) + // ========== 连接和断开 ========== bool NetworkManager::connectToCamera(const QString &ip, int controlPort, int dataPort) { @@ -152,9 +153,11 @@ void NetworkManager::onReadyRead() quint16 senderPort; m_dataSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort); - // 临时添加日志以诊断问题 + // 只打印前5个包的详细信息 if (packetCount < 5) { - qDebug() << "[NetworkManager] Received packet" << packetCount << "from" << sender.toString() << ":" << senderPort << "size:" << datagram.size(); + // qDebug() << "[NetworkManager] Packet" << packetCount + // << "from" << sender.toString() << ":" << senderPort + // << "size:" << datagram.size() << "bytes"; } packetCount++; @@ -164,6 +167,11 @@ void NetworkManager::onReadyRead() // 仍然发出原始数据信号（用于调试） emit dataReceived(datagram); } + + // 每1000个包打印一次统计（减少日志量） + if (packetCount % 1000 == 0) { + // qDebug() << "[NetworkManager] Total packets received:" << packetCount; + } } void NetworkManager::onError(QAbstractSocket::SocketError socketError) diff --git a/src/core/NetworkManager.h b/src/core/NetworkManager.h index e83d3ad..cd33e8f 100644 --- a/src/core/NetworkManager.h +++ b/src/core/NetworkManager.h @@ -36,6 +36,7 @@ signals: void dataReceived(const QByteArray &data); void imageReceived(const QImage &image, uint32_t blockId); void depthDataReceived(const QByteArray &depthData, uint32_t blockId); + void pointCloudDataReceived(const QByteArray &cloudData, uint32_t blockId); private slots: void onReadyRead(); diff --git a/src/core/PointCloudProcessor.cpp b/src/core/PointCloudProcessor.cpp index f836edd..a5e371c 100644 --- a/src/core/PointCloudProcessor.cpp +++ b/src/core/PointCloudProcessor.cpp @@ -1,6 +1,11 @@ #include "core/PointCloudProcessor.h" #include #include +#include + +#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::Ptr cloud(new pcl::PointCloud); + 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(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(m_imageWidth - 1 - col); // 左右翻转 + float y = static_cast(m_imageHeight - 1 - row); // 上下翻转 + float z = static_cast(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(cloudShort[i * 3 + 0]) * m_zScale; + cloud->points[i].y = static_cast(cloudShort[i * 3 + 1]) * m_zScale; + cloud->points[i].z = static_cast(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) { diff --git a/src/gui/MainWindow.cpp b/src/gui/MainWindow.cpp index 371e553..e867ceb 100644 --- a/src/gui/MainWindow.cpp +++ b/src/gui/MainWindow.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -48,9 +49,12 @@ MainWindow::MainWindow(QWidget *parent) , m_autoSaveOnNextFrame(false) , m_currentPointCloud(new pcl::PointCloud()) , m_currentFrameId(0) - , m_frameCount(0) - , m_totalFrameCount(0) - , m_currentFps(0.0) + , m_depthFrameCount(0) + , m_totalDepthFrameCount(0) + , m_currentDepthFps(0.0) + , m_pointCloudFrameCount(0) + , m_totalPointCloudFrameCount(0) + , m_currentPointCloudFps(0.0) { setupUI(); setupConnections(); @@ -74,6 +78,15 @@ MainWindow::MainWindow(QWidget *parent) MainWindow::~MainWindow() { + // 在退出前发送STOP命令，停止下位机采集 + if (m_isConnected && m_networkManager) { + qDebug() << "程序退出，发送STOP命令到下位机"; + m_networkManager->sendStopCommand(); + + // 等待一小段时间确保命令发送完成 + QThread::msleep(100); + } + saveSettings(); } @@ -427,17 +440,20 @@ void MainWindow::setupUI() QGroupBox *statsGroup = new QGroupBox("统计信息", m_controlPanel); QVBoxLayout *statsLayout = new QVBoxLayout(statsGroup); - m_fpsLabel = new QLabel("帧率: 0.0 fps", statsGroup); + m_depthFpsLabel = new QLabel("红外图帧率: 0.0 fps", statsGroup); + m_pointCloudFpsLabel = new QLabel("点云帧率: 0.0 fps", statsGroup); m_resolutionLabel = new QLabel("分辨率: 1224 x 1024", statsGroup); m_queueLabel = new QLabel("接收帧数: 0", statsGroup); // 设置统计标签样式 - 移除固定颜色以支持深色模式 QString statsLabelStyle = "QLabel { font-size: 10pt; padding: 4px; }"; - m_fpsLabel->setStyleSheet(statsLabelStyle); + m_depthFpsLabel->setStyleSheet(statsLabelStyle); + m_pointCloudFpsLabel->setStyleSheet(statsLabelStyle); m_resolutionLabel->setStyleSheet(statsLabelStyle); m_queueLabel->setStyleSheet(statsLabelStyle); - statsLayout->addWidget(m_fpsLabel); + statsLayout->addWidget(m_depthFpsLabel); + statsLayout->addWidget(m_pointCloudFpsLabel); statsLayout->addWidget(m_resolutionLabel); statsLayout->addWidget(m_queueLabel); @@ -472,6 +488,7 @@ void MainWindow::setupConnections() // GVSP数据信号连接（从NetworkManager） connect(m_networkManager.get(), &NetworkManager::imageReceived, this, &MainWindow::onImageReceived); connect(m_networkManager.get(), &NetworkManager::depthDataReceived, this, &MainWindow::onDepthDataReceived); + connect(m_networkManager.get(), &NetworkManager::pointCloudDataReceived, this, &MainWindow::onPointCloudDataReceived); // 点云处理信号连接 connect(m_pointCloudProcessor.get(), &PointCloudProcessor::pointCloudReady, this, &MainWindow::onPointCloudReady); @@ -712,26 +729,26 @@ void MainWindow::onImageReceived(const QImage &image, uint32_t blockId) m_currentImage = image; m_currentFrameId = blockId; - // 计算FPS和累计帧数 - m_frameCount++; - m_totalFrameCount++; + // 计算深度图FPS和累计帧数 + m_depthFrameCount++; + m_totalDepthFrameCount++; QDateTime currentTime = QDateTime::currentDateTime(); - if (m_lastFrameTime.isValid()) { - qint64 elapsed = m_lastFrameTime.msecsTo(currentTime); + if (m_lastDepthFrameTime.isValid()) { + qint64 elapsed = m_lastDepthFrameTime.msecsTo(currentTime); if (elapsed >= 1000) { // 每秒更新一次FPS - m_currentFps = (m_frameCount * 1000.0) / elapsed; - m_frameCount = 0; - m_lastFrameTime = currentTime; + m_currentDepthFps = (m_depthFrameCount * 1000.0) / elapsed; + m_depthFrameCount = 0; + m_lastDepthFrameTime = currentTime; updateStatistics(); } } else { - m_lastFrameTime = currentTime; + m_lastDepthFrameTime = currentTime; } - // 将图像显示在UI上 + // 将图像显示在UI上（使用快速缩放） if (m_imageDisplay) { QPixmap pixmap = QPixmap::fromImage(image); - m_imageDisplay->setPixmap(pixmap.scaled(m_imageDisplay->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation)); + m_imageDisplay->setPixmap(pixmap.scaled(m_imageDisplay->size(), Qt::KeepAspectRatio, Qt::FastTransformation)); } } @@ -745,6 +762,16 @@ void MainWindow::onDepthDataReceived(const QByteArray &depthData, uint32_t block m_pointCloudProcessor->processDepthData(depthData, blockId); } +void MainWindow::onPointCloudDataReceived(const QByteArray &cloudData, uint32_t blockId) +{ + // qDebug() << "[MainWindow] Point cloud data received: Block" << blockId << "Size:" << cloudData.size() << "bytes"; + + // 使用QtConcurrent在后台线程处理点云数据 + QtConcurrent::run([this, cloudData, blockId]() { + m_pointCloudProcessor->processPointCloudData(cloudData, blockId); + }); +} + void MainWindow::onPointCloudReady(pcl::PointCloud::Ptr cloud, uint32_t blockId) { // 注释掉频繁的日志输出 @@ -753,6 +780,22 @@ void MainWindow::onPointCloudReady(pcl::PointCloud::Ptr cloud, ui // 保存当前点云用于拍照 m_currentPointCloud = cloud; + // 计算点云FPS和累计帧数 + m_pointCloudFrameCount++; + m_totalPointCloudFrameCount++; + QDateTime currentTime = QDateTime::currentDateTime(); + if (m_lastPointCloudFrameTime.isValid()) { + qint64 elapsed = m_lastPointCloudFrameTime.msecsTo(currentTime); + if (elapsed >= 1000) { // 每秒更新一次FPS + m_currentPointCloudFps = (m_pointCloudFrameCount * 1000.0) / elapsed; + m_pointCloudFrameCount = 0; + m_lastPointCloudFrameTime = currentTime; + updateStatistics(); + } + } else { + m_lastPointCloudFrameTime = currentTime; + } + // 更新点云显示 if (m_pointCloudWidget) { m_pointCloudWidget->updatePointCloud(cloud); @@ -1093,14 +1136,21 @@ void MainWindow::onSaveLogClicked() // ========== 统计信息更新 ========== void MainWindow::updateStatistics() { - // 更新帧率 - if (m_fpsLabel) { - m_fpsLabel->setText(QString("帧率: %1 fps").arg(m_currentFps, 0, 'f', 1)); + // 更新红外图帧率 + if (m_depthFpsLabel) { + m_depthFpsLabel->setText(QString("红外图帧率: %1 fps").arg(m_currentDepthFps, 0, 'f', 1)); } - // 更新接收帧数（显示累计总数） + // 更新点云帧率 + if (m_pointCloudFpsLabel) { + m_pointCloudFpsLabel->setText(QString("点云帧率: %1 fps").arg(m_currentPointCloudFps, 0, 'f', 1)); + } + + // 更新接收帧数（显示红外图和点云的累计总数） if (m_queueLabel) { - m_queueLabel->setText(QString("接收帧数: %1").arg(m_totalFrameCount)); + m_queueLabel->setText(QString("接收帧数: 红外%1 点云%2") + .arg(m_totalDepthFrameCount) + .arg(m_totalPointCloudFrameCount)); } } diff --git a/src/gui/MainWindow.h b/src/gui/MainWindow.h index 98ffe02..6b50c2a 100644 --- a/src/gui/MainWindow.h +++ b/src/gui/MainWindow.h @@ -52,6 +52,7 @@ private slots: // GVSP数据处理槽函数 void onImageReceived(const QImage &image, uint32_t blockId); void onDepthDataReceived(const QByteArray &depthData, uint32_t blockId); + void onPointCloudDataReceived(const QByteArray &cloudData, uint32_t blockId); void onPointCloudReady(pcl::PointCloud::Ptr cloud, uint32_t blockId); // 设备扫描槽函数 @@ -140,7 +141,8 @@ private: QListWidget *m_deviceList; // 统计信息控件 - QLabel *m_fpsLabel; + QLabel *m_depthFpsLabel; + QLabel *m_pointCloudFpsLabel; QLabel *m_resolutionLabel; QLabel *m_queueLabel; @@ -149,11 +151,17 @@ private: QPushButton *m_clearLogBtn; QPushButton *m_saveLogBtn; - // 统计数据 - QDateTime m_lastFrameTime; - int m_frameCount; - int m_totalFrameCount; - double m_currentFps; + // 统计数据 - 深度图 + QDateTime m_lastDepthFrameTime; + int m_depthFrameCount; + int m_totalDepthFrameCount; + double m_currentDepthFps; + + // 统计数据 - 点云 + QDateTime m_lastPointCloudFrameTime; + int m_pointCloudFrameCount; + int m_totalPointCloudFrameCount; + double m_currentPointCloudFps; }; #endif // MAINWINDOW_H diff --git a/src/gui/PointCloudGLWidget.cpp b/src/gui/PointCloudGLWidget.cpp index 063d009..3668a38 100644 --- a/src/gui/PointCloudGLWidget.cpp +++ b/src/gui/PointCloudGLWidget.cpp @@ -224,11 +224,11 @@ void PointCloudGLWidget::updatePointCloud(pcl::PointCloud::Ptr cl // 添加调试日志 static int updateCount = 0; if (updateCount < 3 || updateCount % 100 == 0) { - qDebug() << "[PointCloudGLWidget] Update" << updateCount << "- Points:" << m_pointCount - << "Total cloud size:" << cloud->size(); - qDebug() << " X range:" << minX << "to" << maxX; - qDebug() << " Y range:" << minY << "to" << maxY; - qDebug() << " Z range:" << minZ << "to" << maxZ; + // qDebug() << "[PointCloudGLWidget] Update" << updateCount << "- Points:" << m_pointCount + // << "Total cloud size:" << cloud->size(); + // qDebug() << " X range:" << minX << "to" << maxX; + // qDebug() << " Y range:" << minY << "to" << maxY; + // qDebug() << " Z range:" << minZ << "to" << maxZ; } updateCount++;