GrabBag/App/BeltTearing/BeltTearingServer/PointCloudImageUtils.cpp

#include "PointCloudImageUtils.h"
#include <QPainter>
#include <cmath>
#include <algorithm>
#include <limits>
#include "VrLog.h"

#include "VrTimeUtils.h"
#include "beltTearingDetection_Export.h"

#ifndef PI
#define PI 3.14159265358979323846
#endif

QImage PointCloudImageUtils::GeneratePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                                     const std::vector<SSG_beltTearingInfo>& beltTearings,
                                                     int imageWidth, int imageHeight)
{
    if (scanLines.empty() || imageWidth <= 0 || imageHeight <= 0) {
        LOG_WARNING("Invalid input parameters: scanLines.size()=%d, imageWidth=%d, imageHeight=%d\n", scanLines.size(), imageWidth, imageHeight);
        return QImage();
    }

    // 快速计算点云范围
    double xMin, xMax, yMin, yMax;
    if (!CalculateRangeFast(scanLines, xMin, xMax, yMin, yMax)) {
        // LOG_WARNING("No valid points found in scan lines, arg=%d count=%d\n", scanLines.size(), scanLines.size() ? scanLines[0].size() : 0);
        return QImage();
    }

    // 检查范围是否有效
    if (xMax <= xMin || yMax <= yMin) {
        LOG_WARNING("Invalid point cloud range: xRange=%.2f, yRange=%.2f\n", xMax - xMin, yMax - yMin);
        return QImage();
    }

    // 创建图像
    QImage image(imageWidth, imageHeight, QImage::Format_RGB888);
    if (image.isNull()) {
        LOG_ERROR("Failed to create QImage with size %dx%d\n", imageWidth, imageHeight);
        return QImage();
    }

    image.fill(Qt::black);

    // 直接绘制到图像数据，避免QPainter开销
    DrawPointCloudDirect(image, scanLines, xMin, xMax, yMin, yMax, imageWidth, imageHeight);

    // 绘制检测结果（如果需要）
    QPainter painter(&image);
    
    if (!beltTearings.empty()) {
        DrawBeltTearingResults(painter, beltTearings, xMin, xMax, yMin, yMax, imageWidth, imageHeight);
    }
    
    return image;
}

bool PointCloudImageUtils::CalculateRangeFast(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                             double& xMin, double& xMax, double& yMin, double& yMax)
{
    if (scanLines.empty()) {
        // 设置默认范围以避免无效计算
        xMin = xMax = yMin = yMax = 0.0;
        return false;
    }

    // 使用局部变量减少内存写入
    double localXMin = std::numeric_limits<double>::max();
    double localXMax = std::numeric_limits<double>::lowest();
    double localYMin = std::numeric_limits<double>::max();
    double localYMax = std::numeric_limits<double>::lowest();

    bool hasValidPoints = false;

    // 使用const引用减少拷贝开销
    for (const auto& scanLine : scanLines) {
        const SVzNL3DPosition* points = scanLine.data();
        const size_t count = scanLine.size();

        // 使用指针遍历，减少边界检查
        for (size_t i = 0; i < count; ++i) {
            const auto& point = points[i];

            // ARM兼容的Z值检查 - 避免位操作
            if (point.pt3D.z < 1e-4) continue;

            hasValidPoints = true;

            // 使用局部变量缓存坐标值
            const double x = point.pt3D.x;
            const double y = point.pt3D.y;

            // 分支预测友好的比较 - 使用条件运算符
            localXMin = (x < localXMin) ? x : localXMin;
            localXMax = (x > localXMax) ? x : localXMax;
            localYMin = (y < localYMin) ? y : localYMin;
            localYMax = (y > localYMax) ? y : localYMax;
        }
    }

    // 最后一次性写入结果
    xMin = localXMin;
    xMax = localXMax;
    yMin = localYMin;
    yMax = localYMax;

    return hasValidPoints;
}

void PointCloudImageUtils::DrawPointCloudDirect(QImage& image,
                                               const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                               double xMin, double xMax, double yMin, double yMax,
                                               int imageWidth, int imageHeight)
{
    if (scanLines.empty()) return;

    // 检查范围是否有效，避免除零错误
    if (xMax <= xMin || yMax <= yMin) {
        LOG_WARNING("Invalid range for drawing: xMin=%f, xMax=%f, yMin=%f, yMax=%f\n", xMin, xMax, yMin, yMax);
        return;
    }

    // 检查图像尺寸是否有效
    if (imageWidth <= 0 || imageHeight <= 0) {
        LOG_WARNING("Invalid image dimensions: width=%d, height=%d\n", imageWidth, imageHeight);
        return;
    }

    // 预计算缩放因子和偏移量
    const double xScale = imageWidth / (xMax - xMin);
    const double yScale = imageHeight / (yMax - yMin);

    // 验证图像格式是否为RGB888
    if (image.format() != QImage::Format_RGB888) {
        LOG_WARNING("Image format is not RGB888, converting...\n");
        image = image.convertToFormat(QImage::Format_RGB888);
    }

    // 直接访问图像数据
    uchar* imageData = image.bits();
    if (!imageData) {
        LOG_ERROR("Failed to get image data pointer\n");
        return;
    }

    const int bytesPerLine = image.bytesPerLine();
    const int maxX = imageWidth - 1;
    const int maxY = imageHeight - 1;

    // 预计算常量
    constexpr uchar grayValue = 150;

    for (const auto& scanLine : scanLines) {
        const SVzNL3DPosition* points = scanLine.data();
        const size_t count = scanLine.size();

        // 使用指针遍历减少边界检查
        for (size_t i = 0; i < count; ++i) {
            const auto& point = points[i];

            // ARM兼容的Z值检查 - 避免位操作
            if (point.pt3D.z < 1e-4) continue;

            // 快速坐标转换
            const int px = static_cast<int>((point.pt3D.x - xMin) * xScale);
            const int py = static_cast<int>((point.pt3D.y - yMin) * yScale);

            // 快速边界检查 - 使用无符号比较
            if (static_cast<unsigned>(px) <= static_cast<unsigned>(maxX) &&
                static_cast<unsigned>(py) <= static_cast<unsigned>(maxY)) {

                // ARM安全的像素写入 - 确保内存对齐
                const size_t pixelOffset = static_cast<size_t>(py) * bytesPerLine + static_cast<size_t>(px) * 3;
                if (pixelOffset + 2 < static_cast<size_t>(image.sizeInBytes())) {
                    uchar* pixel = imageData + pixelOffset;
                    pixel[0] = grayValue; // R
                    pixel[1] = grayValue; // G
                    pixel[2] = grayValue; // B
                }
            }
        }
    }
}

void PointCloudImageUtils::DrawLapWeldResults(QPainter& painter,
                                             const std::vector<std::vector<SVzNL3DPoint>>& weldResults,
                                             double xMin, double xMax, double yMin, double yMax,
                                             int imageWidth, int imageHeight)
{
    if (weldResults.empty()) return;

    // 检查范围是否有效，避免除零错误
    if (xMax <= xMin || yMax <= yMin) {
        LOG_WARNING("Invalid range for drawing: xMin=%f, xMax=%f, yMin=%f, yMax=%f\n", xMin, xMax, yMin, yMax);
        return;
    }

    // 检查图像尺寸是否有效
    if (imageWidth <= 0 || imageHeight <= 0) {
        LOG_WARNING("Invalid image dimensions: width=%d, height=%d\n", imageWidth, imageHeight);
        return;
    }

    double xScale = imageWidth / (xMax - xMin);
    double yScale = imageHeight / (yMax - yMin);

    // 使用不同颜色绘制每条焊缝
    QColor weldColors[] = {
        QColor(255, 0, 0),     // 红色
        QColor(0, 255, 0),     // 绿色
        QColor(0, 0, 255),     // 蓝色
        QColor(255, 255, 0),   // 黄色
        QColor(255, 0, 255),   // 紫色
        QColor(0, 255, 255),   // 青色
        QColor(255, 128, 0),   // 橙色
        QColor(128, 255, 0)    // 浅绿色
    };
    int numColors = sizeof(weldColors) / sizeof(weldColors[0]);

    for (size_t i = 0; i < weldResults.size(); i++) {
        const auto& weldLine = weldResults[i];
        if (weldLine.empty()) continue;

        QColor weldColor = weldColors[i % numColors];
        painter.setPen(QPen(weldColor, 3));

        // 绘制焊缝线段
        for (size_t j = 1; j < weldLine.size(); j++) {
            int px1 = (int)((weldLine[j-1].x - xMin) * xScale);
            int py1 = (int)((weldLine[j-1].y - yMin) * yScale);
            int px2 = (int)((weldLine[j].x - xMin) * xScale);
            int py2 = (int)((weldLine[j].y - yMin) * yScale);

            if (px1 >= 0 && px1 < imageWidth && py1 >= 0 && py1 < imageHeight &&
                px2 >= 0 && px2 < imageWidth && py2 >= 0 && py2 < imageHeight) {
                painter.drawLine(px1, py1, px2, py2);
            }
        }

        // 在起点和终点绘制标记
        if (!weldLine.empty()) {
            // 起点标记 - 圆形
            int startX = (int)((weldLine[0].x - xMin) * xScale);
            int startY = (int)((weldLine[0].y - yMin) * yScale);
            if (startX >= 0 && startX < imageWidth && startY >= 0 && startY < imageHeight) {
                painter.setPen(QPen(weldColor, 2));
                painter.setBrush(QBrush(weldColor));
                painter.drawEllipse(startX - 5, startY - 5, 10, 10);
            }

            // 终点标记 - 方形
            int endX = (int)((weldLine.back().x - xMin) * xScale);
            int endY = (int)((weldLine.back().y - yMin) * yScale);
            if (endX >= 0 && endX < imageWidth && endY >= 0 && endY < imageHeight) {
                painter.setPen(QPen(weldColor, 2));
                painter.setBrush(QBrush(weldColor));
                painter.drawRect(endX - 4, endY - 4, 8, 8);
            }
        }
    }
}

void PointCloudImageUtils::DrawBeltTearingResults(QPainter& painter,
                                                 const std::vector<SSG_beltTearingInfo>& tearings,
                                                 double xMin, double xMax, double yMin, double yMax,
                                                 int imageWidth, int imageHeight)
{
    if (tearings.empty()) return;

    // 检查范围是否有效，避免除零错误
    if (xMax <= xMin || yMax <= yMin) {
        LOG_WARNING("Invalid range for drawing: xMin=%f, xMax=%f, yMin=%f, yMax=%f\n", xMin, xMax, yMin, yMax);
        return;
    }

    // 检查图像尺寸是否有效
    if (imageWidth <= 0 || imageHeight <= 0) {
        LOG_WARNING("Invalid image dimensions: width=%d, height=%d\n", imageWidth, imageHeight);
        return;
    }

    double xScale = imageWidth / (xMax - xMin);
    double yScale = imageHeight / (yMax - yMin);

    // 使用10种协调的颜色根据撕裂ID进行绘制
    QColor tearColors[] = {
        QColor(255, 105, 97),   // 珊瑚红
        QColor(255, 160, 122),  // 浅鲑鱼色
        QColor(173, 216, 230),  // 浅蓝色
        QColor(144, 238, 144),  // 浅绿色
        QColor(255, 182, 193),  // 浅粉色
        QColor(221, 160, 221),  // 梅花色
        QColor(255, 215, 0),    // 金色
        QColor(240, 128, 128),  // 玫瑰色
        QColor(135, 206, 250),  // 天蓝色
        QColor(127, 255, 212)   // 碧绿色
    };
    int numColors = sizeof(tearColors) / sizeof(tearColors[0]);

    for (size_t i = 0; i < tearings.size(); i++) {
        const auto& tearing = tearings[i];
        // 根据撕裂ID选择颜色，确保相同ID使用相同颜色
        QColor tearColor = tearColors[tearing.tearID % numColors];

        // 如果撕裂信息包含点集，则绘制点（每隔一定数量的点绘制一个，减少密度）
#if OUTPUT_TEARING_POINTS
        if (!tearing.pts.empty()) {
            // 每隔3个点绘制一个点，减少点的密度
            const int pointInterval = 3;
            for (size_t idx = 0; idx < tearing.pts.size(); idx += pointInterval) {
                const auto& point = tearing.pts[idx];
                int px = (int)((point.pt3D.x - xMin) * xScale);
                int py = (int)((point.pt3D.y - yMin) * yScale);
                
                if (px >= 0 && px < imageWidth && py >= 0 && py < imageHeight) {
                    // 设置画笔和画刷用于绘制点
                    painter.setPen(QPen(tearColor, 1));  // 减小画笔宽度
                    painter.setBrush(QBrush(tearColor));
                    // 绘制点为小圆圈，减小点的大小
                    painter.drawEllipse(px - 1, py - 1, 2, 2);
                }
            }
        }
#endif
    }
}