GrabBag/App/BeltTearing/BeltTearingServer/PointCloudImageUtils.cpp

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

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

#ifndef PI
#define PI 3.14159265358979323846
#endif

namespace
{
constexpr double kPseudoColorPeriod = 50.0;
constexpr int kPseudoColorMaxCode = 65535;

bool IsValidPoint(const SVzNL3DPosition& point)
{
    return std::isfinite(point.pt3D.x) &&
           std::isfinite(point.pt3D.y) &&
           std::isfinite(point.pt3D.z) &&
           point.pt3D.z >= 1e-4;
}

int DepthToPseudoColorCode(double z)
{
    const double rangeStart = std::floor(z / kPseudoColorPeriod) * kPseudoColorPeriod;
    const double offset = z - rangeStart;
    return (std::max)(0, (std::min)(kPseudoColorMaxCode,
        static_cast<int>(std::round(offset / kPseudoColorPeriod * kPseudoColorMaxCode))));
}

const std::vector<QRgb>& GetPseudoColorLut()
{
    static const std::vector<QRgb> lut = []() {
        std::vector<QRgb> colors(kPseudoColorMaxCode + 1);
        for (int code = 0; code <= kPseudoColorMaxCode; ++code) {
            const double ratio = static_cast<double>(code) / kPseudoColorMaxCode;
            // 期望的色带方向为红->品红->蓝->青->绿->黄->红。
            // 反向遍历HSV色环可使相邻50 mm周期在红色处连续衔接。
            const int hue = static_cast<int>(std::floor(360.0 * (1.0 - ratio))) % 360;
            colors[static_cast<size_t>(code)] = QColor::fromHsv(hue, 255, 255).rgb();
        }
        return colors;
    }();
    return lut;
}

void SetDepthPixel(std::vector<float>& depth, int imageWidth, int imageHeight,
                   int x, int y, double z)
{
    if (x < 0 || x >= imageWidth || y < 0 || y >= imageHeight) {
        return;
    }

    float& current = depth[static_cast<size_t>(y) * imageWidth + x];
    if (std::isfinite(current)) {
        current = static_cast<float>((static_cast<double>(current) + z) * 0.5);
    } else {
        current = static_cast<float>(z);
    }
}

void RasterizeDepthSegment(std::vector<float>& depth, int imageWidth, int imageHeight,
                           int x0, int y0, double z0, int x1, int y1, double z1,
                           int maxGap)
{
    const int dx = std::abs(x1 - x0);
    const int dy = std::abs(y1 - y0);
    const int steps = (std::max)(dx, dy);
    if (steps == 0) {
        SetDepthPixel(depth, imageWidth, imageHeight, x0, y0, z0);
        return;
    }
    if (steps > maxGap) {
        return;
    }

    for (int step = 0; step <= steps; ++step) {
        const double ratio = static_cast<double>(step) / steps;
        const int x = static_cast<int>(std::round(x0 + (x1 - x0) * ratio));
        const int y = static_cast<int>(std::round(y0 + (y1 - y0) * ratio));
        const double z = z0 + (z1 - z0) * ratio;
        SetDepthPixel(depth, imageWidth, imageHeight, x, y, z);
    }
}

struct OrthographicProjection
{
    double scale = 1.0;
    double offsetX = 0.0;
    double offsetY = 0.0;
};

OrthographicProjection MakeOrthographicProjection(double xMin, double xMax,
                                                   double yMin, double yMax,
                                                   int imageWidth, int imageHeight)
{
    OrthographicProjection projection;
    const double xRange = xMax - xMin;
    const double yRange = yMax - yMin;
    projection.scale = (std::min)(
        static_cast<double>(imageWidth - 1) / xRange,
        static_cast<double>(imageHeight - 1) / yRange);
    projection.offsetX = ((imageWidth - 1) - xRange * projection.scale) * 0.5;
    projection.offsetY = ((imageHeight - 1) - yRange * projection.scale) * 0.5;
    return projection;
}
}

QImage PointCloudImageUtils::GeneratePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                                     const std::vector<SSG_beltTearingInfo>& beltTearings,
                                                     int imageWidth, int imageHeight)
{
    if (scanLines.empty() || imageWidth <= 1 || imageHeight <= 1) {
        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();
    }

    // imageWidth/imageHeight是最大输出边界，实际尺寸按物理X/Y范围等比例生成。
    const double xRange = xMax - xMin;
    const double yRange = yMax - yMin;
    if (xRange <= 0.0 || yRange <= 0.0) {
        LOG_WARNING("Invalid point cloud range: xRange=%.2f, yRange=%.2f\n", xRange, yRange);
        return QImage();
    }
    const double outputScale = (std::min)(
        static_cast<double>(imageWidth - 1) / xRange,
        static_cast<double>(imageHeight - 1) / yRange);
    const int outputWidth = (std::max)(2, (std::min)(imageWidth,
        static_cast<int>(std::round(xRange * outputScale)) + 1));
    const int outputHeight = (std::max)(2, (std::min)(imageHeight,
        static_cast<int>(std::round(yRange * outputScale)) + 1));

    QImage image(outputWidth, outputHeight, QImage::Format_RGB888);
    if (image.isNull()) {
        LOG_ERROR("Failed to create QImage with size %dx%d\n", outputWidth, outputHeight);
        return QImage();
    }

    image.fill(Qt::black);

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

    // 绘制检测结果（如果需要）
    QPainter painter(&image);
    
    if (!beltTearings.empty()) {
        DrawBeltTearingResults(painter, beltTearings, xMin, xMax, yMin, yMax, outputWidth, outputHeight);
    }
    
    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];

            if (!IsValidPoint(point)) 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 || imageWidth <= 0 || imageHeight <= 0) {
        LOG_WARNING("Invalid point cloud drawing range or image size\n");
        return;
    }

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

    const OrthographicProjection projection = MakeOrthographicProjection(
        xMin, xMax, yMin, yMax, imageWidth, imageHeight);
    const int maxConnectionGap = (std::max)(8,
        (std::min)(64, (std::max)(imageWidth, imageHeight) / 16));
    const float invalidDepth = std::numeric_limits<float>::quiet_NaN();
    std::vector<float> depth(static_cast<size_t>(imageWidth) * imageHeight, invalidDepth);

    auto toPixel = [&](const SVzNL3DPosition& point, int& px, int& py) {
        px = static_cast<int>(std::round(
            projection.offsetX + (point.pt3D.x - xMin) * projection.scale));
        py = static_cast<int>(std::round(
            projection.offsetY + (point.pt3D.y - yMin) * projection.scale));
    };

    for (size_t lineIndex = 0; lineIndex < scanLines.size(); ++lineIndex) {
        const auto& scanLine = scanLines[lineIndex];
        bool hasPrevious = false;
        int previousX = 0;
        int previousY = 0;
        double previousZ = 0.0;

        for (const auto& point : scanLine) {
            if (!IsValidPoint(point)) {
                hasPrevious = false;
                continue;
            }

            int px = 0;
            int py = 0;
            toPixel(point, px, py);
            const double z = point.pt3D.z;
            SetDepthPixel(depth, imageWidth, imageHeight, px, py, z);

            if (hasPrevious) {
                RasterizeDepthSegment(depth, imageWidth, imageHeight,
                    previousX, previousY, previousZ, px, py, z, maxConnectionGap);
            }

            previousX = px;
            previousY = py;
            previousZ = z;
            hasPrevious = true;
        }

        // 连接相邻扫描线中对应采样点，补齐 X-Y 网格间的像素空洞。
        if (lineIndex > 0) {
            const auto& previousLine = scanLines[lineIndex - 1];
            const size_t commonCount = (std::min)(previousLine.size(), scanLine.size());
            for (size_t pointIndex = 0; pointIndex < commonCount; ++pointIndex) {
                const auto& previousPoint = previousLine[pointIndex];
                const auto& currentPoint = scanLine[pointIndex];
                if (!IsValidPoint(previousPoint) || !IsValidPoint(currentPoint)) {
                    continue;
                }

                int previousPixelX = 0;
                int previousPixelY = 0;
                int currentPixelX = 0;
                int currentPixelY = 0;
                toPixel(previousPoint, previousPixelX, previousPixelY);
                toPixel(currentPoint, currentPixelX, currentPixelY);
                RasterizeDepthSegment(depth, imageWidth, imageHeight,
                    previousPixelX, previousPixelY, previousPoint.pt3D.z,
                    currentPixelX, currentPixelY, currentPoint.pt3D.z,
                    maxConnectionGap);
            }
        }
    }

    uchar* imageData = image.bits();
    if (!imageData) {
        LOG_ERROR("Failed to get image data pointer\n");
        return;
    }

    const std::vector<QRgb>& pseudoColorLut = GetPseudoColorLut();
    for (int y = 0; y < imageHeight; ++y) {
        for (int x = 0; x < imageWidth; ++x) {
            const float z = depth[static_cast<size_t>(y) * imageWidth + x];
            if (!std::isfinite(z)) {
                continue;
            }

            const QRgb color = pseudoColorLut[static_cast<size_t>(DepthToPseudoColorCode(z))];
            uchar* pixel = imageData + static_cast<size_t>(y) * image.bytesPerLine() + static_cast<size_t>(x) * 3;
            pixel[0] = static_cast<uchar>(qRed(color));
            pixel[1] = static_cast<uchar>(qGreen(color));
            pixel[2] = static_cast<uchar>(qBlue(color));
        }
    }
}

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;
    }

    const OrthographicProjection projection = MakeOrthographicProjection(
        xMin, xMax, yMin, yMax, imageWidth, imageHeight);

    // 使用不同颜色绘制每条焊缝
    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 = static_cast<int>(std::round(projection.offsetX +
                (weldLine[j-1].x - xMin) * projection.scale));
            int py1 = static_cast<int>(std::round(projection.offsetY +
                (weldLine[j-1].y - yMin) * projection.scale));
            int px2 = static_cast<int>(std::round(projection.offsetX +
                (weldLine[j].x - xMin) * projection.scale));
            int py2 = static_cast<int>(std::round(projection.offsetY +
                (weldLine[j].y - yMin) * projection.scale));

            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 = static_cast<int>(std::round(projection.offsetX +
                (weldLine[0].x - xMin) * projection.scale));
            int startY = static_cast<int>(std::round(projection.offsetY +
                (weldLine[0].y - yMin) * projection.scale));
            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 = static_cast<int>(std::round(projection.offsetX +
                (weldLine.back().x - xMin) * projection.scale));
            int endY = static_cast<int>(std::round(projection.offsetY +
                (weldLine.back().y - yMin) * projection.scale));
            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;
    }

    const OrthographicProjection projection = MakeOrthographicProjection(
        xMin, xMax, yMin, yMax, imageWidth, imageHeight);

    for (const auto& tearing : tearings) {

        int left = static_cast<int>(std::round(projection.offsetX +
            (tearing.roi.left - xMin) * projection.scale));
        int right = static_cast<int>(std::round(projection.offsetX +
            (tearing.roi.right - xMin) * projection.scale));
        int top = static_cast<int>(std::round(projection.offsetY +
            (tearing.roi.top - yMin) * projection.scale));
        int bottom = static_cast<int>(std::round(projection.offsetY +
            (tearing.roi.bottom - yMin) * projection.scale));
        if (left > right) std::swap(left, right);
        if (top > bottom) std::swap(top, bottom);
        left = (std::max)(0, (std::min)(imageWidth - 1, left));
        right = (std::max)(0, (std::min)(imageWidth - 1, right));
        top = (std::max)(0, (std::min)(imageHeight - 1, top));
        bottom = (std::max)(0, (std::min)(imageHeight - 1, bottom));
        if (right > left && bottom > top) {
            const QRect roiRect(left, top, right - left, bottom - top);
            painter.setBrush(Qt::NoBrush);
            painter.setPen(QPen(Qt::green, 2));
            painter.drawRect(roiRect.adjusted(-1, -1, 1, 1));
            painter.setPen(QPen(Qt::red, 2));
            painter.drawRect(roiRect);
        }
    }
}