GrabBag/CloudUtils/Src/PointCloudImageUtils.cpp

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

#ifndef PI
#define PI 3.14159265358979323846
#endif

QImage PointCloudImageUtils::GeneratePointCloudImage(SVzNL3DLaserLine* scanData, 
                                                     int lineNum, 
                                                     const std::vector<SSG_peakRgnInfo>& objOps)
{
    if (!scanData || lineNum <= 0) {
        return QImage();  // 返回空图像
    }
    
    // 统计X和Y的范围
    double xMin = 0.0, xMax = -1.0;
    double yMin = 0.0, yMax = -1.0;
    CalculatePointCloudRange(scanData, lineNum, xMin, xMax, yMin, yMax);
    
    // 检查范围是否有效
    if (xMax < xMin || yMax < yMin) {
        return QImage();  // 返回空图像
    }

    // 创建图像
    int imgRows = 992;
	int imgCols = 1056;
    int x_skip = 16;
    int y_skip = 16;
    
    // 计算投影比例
    double y_rows = (double)(imgRows - y_skip *2);
	double x_cols = (double)(imgCols - x_skip *2);
	//计算投影比例
	double x_scale = (xMax - xMin) / x_cols;
	double y_scale = (yMax - yMin) / y_rows;
	if (x_scale < y_scale)
		x_scale = y_scale;
	else
		y_scale = x_scale;
    
    QImage image(imgCols, imgRows, QImage::Format_RGB888);
    image.fill(Qt::black);
    QPainter painter(&image);
    
    // 绘制点云
    for (int line = 0; line < lineNum; line++) {
        for (int i = 0; i < scanData[line].nPositionCnt; i++) {
            SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
            if (pt3D->pt3D.z < 1e-4) continue;

            // 解析点索引信息
            int vType = pt3D->nPointIdx & 0xff;
            int hType = vType >> 4;
            int objId = (pt3D->nPointIdx >> 16) & 0xff;
            vType = vType & 0x0f;
            
            // 根据线特征类型确定颜色和大小
            QColor pointColor;
            int pointSize = 1;
            GetLineFeatureStyle(vType, hType, objId, pointColor, pointSize);

            int px = (int)((pt3D->pt3D.x - xMin) / x_scale + x_skip);
            int py = (int)((pt3D->pt3D.y - yMin) / y_scale + y_skip);
            
            if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
                painter.setPen(QPen(pointColor, 1));
                painter.drawPoint(px, py);
            }
        }
    }
    
    // 绘制检测目标和方向线
    DrawDetectionTargets(painter, objOps, xMin, x_scale, x_skip, yMin, y_scale, y_skip, imgCols, imgRows);
    
    return image;
}

QImage PointCloudImageUtils::GeneratePointCloudImage(SVzNLXYZRGBDLaserLine* scanData, 
                                                     int lineNum, 
                                                     const std::vector<SSG_peakOrienRgnInfo>& objOps)
{
    
    int imgRows = 992;
    int imgCols = 1056;
    QImage image(imgCols, imgRows, QImage::Format_RGB888);
    image.fill(Qt::black);
    

    if (!scanData || lineNum <= 0) {
        return image;  // 返回空图像
    }
    
    // 统计X和Y的范围 - 参考_RGBDto2DImage函数
    double xMin = 0.0, xMax = -1.0;
    double yMin = 0.0, yMax = -1.0;
    
    for (int line = 0; line < lineNum; line++) {
        for (int i = 0; i < scanData[line].nPointCnt; i++) {
            SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i];
            if (pt3D->z < 1e-4) continue;

            // 更新X范围
            if (xMax < xMin) {
                xMin = xMax = pt3D->x;
            } else {
                if (xMin > pt3D->x) xMin = pt3D->x;
                if (xMax < pt3D->x) xMax = pt3D->x;
            }
            
            // 更新Y范围
            if (yMax < yMin) {
                yMin = yMax = pt3D->y;
            } else {
                if (yMin > pt3D->y) yMin = pt3D->y;
                if (yMax < pt3D->y) yMax = pt3D->y;
            }
        }
    }
    
    // 检查范围是否有效
    if (xMax < xMin || yMax < yMin) {
        return image;  // 返回空图像
    }

    // 创建图像 - 参考_RGBDto2DImage函数的尺寸和偏移
    int x_skip = 16;
    int y_skip = 16;
    double y_rows = (double)(imgRows - y_skip * 2);
    double x_cols = (double)(imgCols - x_skip * 2);
    
    // 计算投影比例 - 参考_RGBDto2DImage函数的比例计算
    double x_scale = (xMax - xMin) / x_cols;
    double y_scale = (yMax - yMin) / y_rows;
    if (x_scale < y_scale)
        x_scale = y_scale;
    else
        y_scale = x_scale;
    
    QPainter painter(&image);
    
    // 绘制点云 - 参考_RGBDto2DImage函数的绘制方式
    for (int line = 0; line < lineNum; line++) {
        for (int i = 0; i < scanData[line].nPointCnt; i++) {
            SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i];
            if (pt3D->z < 1e-4) continue;

            // 解析RGB颜色 - 参考_RGBDto2DImage函数的解析方式
            int nRGB = pt3D->nRGB;
            int r = nRGB & 0xff;
            nRGB >>= 8;
            int g = nRGB & 0xff;
            nRGB >>= 8;
            int b = nRGB & 0xff;
            
            QColor pointColor(r, g, b);

            int px = (int)((pt3D->x - xMin) / x_scale + x_skip);
            int py = (int)((pt3D->y - yMin) / y_scale + y_skip);
            
            if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
                painter.setPen(QPen(pointColor, 1));
                painter.drawPoint(px, py);
            }
        }
    }
    
    // 绘制检测目标和方向线
    DrawDetectionTargets(painter, objOps, xMin, x_scale, x_skip, yMin, y_scale, y_skip, imgCols, imgRows);
    
    return image;
}

// 新的点云图像生成函数 - 基于X、Y范围创建图像
QImage PointCloudImageUtils::GeneratePointCloudImage(SVzNLXYZRGBDLaserLine* scanData, int lineNum)
{
    if (!scanData || lineNum <= 0) {
        return QImage();
    }
    
    // 1. 遍历X, Y的范围，计算最小和最大值
    double xMin = std::numeric_limits<double>::max();
    double xMax = std::numeric_limits<double>::lowest();
    double yMin = std::numeric_limits<double>::max();
    double yMax = std::numeric_limits<double>::lowest();
    
    for (int i = 0; i < lineNum; i++) {
        if (scanData[i].p3DPoint && scanData[i].nPointCnt > 0) {
            for (int j = 0; j < scanData[i].nPointCnt; j++) {
                const SVzNLPointXYZRGBA& point = scanData[i].p3DPoint[j];
                
                // 更新X范围
                if (point.x < xMin) xMin = point.x;
                if (point.x > xMax) xMax = point.x;
                
                // 更新Y范围
                if (point.y < yMin) yMin = point.y;
                if (point.y > yMax) yMax = point.y;
            }
        }
    }
    
    // 检查范围是否有效
    if (xMin >= xMax || yMin >= yMax) {
        return QImage();
    }
    
    // 2. 根据范围创建图像大小
    // 设置图像分辨率，可以根据需要调整
    const int imageWidth = xMax - xMin;
    const int imageHeight = yMax - yMin;

    
    // 3. 创建默认黑底图像
    QImage image(imageWidth, imageHeight, QImage::Format_RGB888);
    image.fill(Qt::black);
    
    // 4. 遍历点云数据，将3D X, Y对应的颜色值赋值给图像
    for (int i = 0; i < lineNum; i++) {
        if (scanData[i].p3DPoint && scanData[i].nPointCnt > 0) {
            for (int j = 0; j < scanData[i].nPointCnt; j++) {
                const SVzNLPointXYZRGBA& point = scanData[i].p3DPoint[j];
                
                // 将3D坐标转换为图像坐标
                int imageX = static_cast<int>((point.x - xMin));
                int imageY = static_cast<int>((point.y - yMin));
                
                // 确保坐标在图像范围内
                if (imageX >= 0 && imageX < imageWidth && 
                    imageY >= 0 && imageY < imageHeight) {
                    
                    // 解析RGB颜色 - 参考_RGBDto2DImage函数的解析方式
                    int nRGB = point.nRGB;
                    int r = nRGB & 0xff;
                    nRGB >>= 8;
                    int g = nRGB & 0xff;
                    nRGB >>= 8;
                    int b = nRGB & 0xff;
                    
                    QColor pointColor(r, g, b);
                    // 设置图像像素颜色
                    image.setPixel(imageX, imageY, pointColor.rgb());
                }
            }
        }
    }
    
    return image;
}

void PointCloudImageUtils::GetLineFeatureStyle(int vType, int hType, int objId, 
                                              QColor& pointColor, int& pointSize)
{
    pointSize = 1;
    
    // 优先根据垂直方向特征设置颜色
    if (LINE_FEATURE_L_JUMP_H2L == vType) {
        pointColor = QColor(255, 97, 0);   // 橙色
        pointSize = 2;
    }
    else if (LINE_FEATURE_L_JUMP_L2H == vType) {
        pointColor = QColor(255, 255, 0);  // 黄色
        pointSize = 2;
    }
    else if (LINE_FEATURE_V_SLOPE == vType) {
        pointColor = QColor(255, 0, 255);  // 紫色
        pointSize = 2;
    }
    else if (LINE_FEATURE_L_SLOPE_H2L == vType) {
        pointColor = QColor(160, 82, 45);  // 褐色
        pointSize = 2;
    }
    else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) {
        pointColor = QColor(255, 0, 0);    // 红色
        pointSize = 2;
    }
    else if (LINE_FEATURE_L_SLOPE_L2H == vType) {
        pointColor = QColor(233, 150, 122); // 浅褐色
        pointSize = 2;
    }
    // 检查水平方向特征
    else if (LINE_FEATURE_L_JUMP_H2L == hType) {
        pointColor = QColor(0, 0, 255);    // 蓝色
        pointSize = 2;
    }
    else if (LINE_FEATURE_L_JUMP_L2H == hType) {
        pointColor = QColor(0, 255, 255);  // 青色
        pointSize = 2;
    }
    else if (LINE_FEATURE_V_SLOPE == hType) {
        pointColor = QColor(0, 255, 0);    // 绿色
        pointSize = 2;
    }
    else if (LINE_FEATURE_L_SLOPE_H2L == hType) {
        pointColor = QColor(85, 107, 47);  // 橄榄绿
        pointSize = 2;
    }
    else if (LINE_FEATURE_L_SLOPE_L2H == hType) {
        pointColor = QColor(0, 255, 154);  // 浅绿色
        pointSize = 2;
    }
    else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) {
        pointColor = QColor(255, 0, 0);    // 红色
        pointSize = 3;
    }
    // 检查是否为目标对象
    else if (objId > 0) {
        pointColor = GetObjectColor(objId);
        pointSize = 1;
    }
    // 默认颜色
    else {
        pointColor = QColor(150, 150, 150);   // 深灰色
        pointSize = 1;
    }
}

QColor PointCloudImageUtils::GetObjectColor(int index)
{
    QColor objColors[8] = {
        QColor(245,222,179), QColor(210,105,30), QColor(240,230,140), QColor(135,206,235),
        QColor(250,235,215), QColor(189,252,201), QColor(221,160,221), QColor(188,143,143)
    };
    
    return objColors[index % 8];
}

void PointCloudImageUtils::CalculatePointCloudRange(SVzNL3DLaserLine* scanData, int lineNum,
                                                   double& xMin, double& xMax, 
                                                   double& yMin, double& yMax)
{
    xMin = 0.0; xMax = -1.0;
    yMin = 0.0; yMax = -1.0;
    
    for (int line = 0; line < lineNum; line++) {
        for (int i = 0; i < scanData[line].nPositionCnt; i++) {
            SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
            if (pt3D->pt3D.z < 1e-4) continue;

            // 更新X范围
            if (xMax < xMin) {
                xMin = xMax = pt3D->pt3D.x;
            } else {
                if (xMin > pt3D->pt3D.x) xMin = pt3D->pt3D.x;
                if (xMax < pt3D->pt3D.x) xMax = pt3D->pt3D.x;
            }
            
            // 更新Y范围
            if (yMax < yMin) {
                yMin = yMax = pt3D->pt3D.y;
            } else {
                if (yMin > pt3D->pt3D.y) yMin = pt3D->pt3D.y;
                if (yMax < pt3D->pt3D.y) yMax = pt3D->pt3D.y;
            }
        }
    }
}

void PointCloudImageUtils::DrawDetectionTargets(QPainter& painter, 
                                               const std::vector<SSG_peakRgnInfo>& objOps,
                                               double xMin, double xScale, int xSkip,
                                               double yMin, double yScale, int ySkip,
                                               int imgCols, int imgRows)
{
    // 绘制检测目标和方向线
    for (size_t i = 0; i < objOps.size(); i++) {
        QColor objColor = (i == 0) ? QColor(255, 0, 0) : QColor(255, 255, 0);
        int size = (i == 0) ? 12 : 8;
        
        int px = (int)((objOps[i].centerPos.x - xMin) / xScale + xSkip);
        int py = (int)((objOps[i].centerPos.y - yMin) / yScale + ySkip);
        
        if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
            // 绘制抓取点圆圈
            painter.setPen(QPen(objColor, 2));
            painter.setBrush(QBrush(objColor));
            painter.drawEllipse(px - size/2, py - size/2, size, size);
            
            // 绘制方向线
            const double deg2rad = PI / 180.0;
            
            // 使用检测目标实际2D尺寸的较大值的一半作为方向线长度
            double maxSize = std::max(objOps[i].objSize.dHeight, objOps[i].objSize.dWidth);
            double R = std::max(20.0, maxSize / 6.0);

            const double yaw = objOps[i].centerPos.z_yaw * deg2rad;
            double cy = cos(yaw); 
            double sy = sin(yaw);
            double x1 = objOps[i].centerPos.x + R * cy; 
            double y1 = objOps[i].centerPos.y - R * sy;
            double x2 = objOps[i].centerPos.x - R * cy; 
            double y2 = objOps[i].centerPos.y + R * sy;
            
            int px1 = (int)((x1 - xMin) / xScale + xSkip);
            int py1 = (int)((y1 - yMin) / yScale + ySkip);
            int px2 = (int)((x2 - xMin) / xScale + xSkip);
            int py2 = (int)((y2 - yMin) / yScale + ySkip);
            
            // 绘制方向线
            painter.setPen(QPen(objColor, 3));
            painter.drawLine(px1, py1, px2, py2);
            
            // 绘制目标编号
            painter.setPen(QPen(Qt::white, 1));
            painter.setFont(QFont("Arial", 15, QFont::Bold));
            painter.drawText(px + 15, py - 10, QString("%1").arg(i + 1));
        }
    }
} 


void PointCloudImageUtils::DrawDetectionTargets(QPainter& painter,
    const std::vector<SSG_peakOrienRgnInfo>& objOps,
    double xMin, double xScale, int xSkip,
    double yMin, double yScale, int ySkip,
    int imgCols, int imgRows)
{
    // 绘制检测目标和方向线 - 参考bagPositioning_test.cpp中的_XOYprojection_RGBD函数
    for (size_t i = 0; i < objOps.size(); i++) {
        QColor objColor = (i == 0) ? QColor(255, 0, 0) : QColor(255, 255, 0);
        int size = (i == 0) ? 20 : 10;

        int px = (int)((objOps[i].centerPos.x - xMin) / xScale + xSkip);
        int py = (int)((objOps[i].centerPos.y - yMin) / yScale + ySkip);

        if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
            // 绘制抓取点圆圈
            painter.setPen(QPen(objColor, 2));
            painter.setBrush(QBrush(objColor));
            painter.drawEllipse(px - size/2, py - size/2, size, size);

            // 绘制方向线 - 参考bagPositioning_test.cpp中的实现
            const double deg2rad = PI / 180.0;

            // 使用检测目标实际2D尺寸的较大值的一半作为方向线长度
            double maxSize = std::max(objOps[i].objSize.dHeight, objOps[i].objSize.dWidth);
            double R = std::max(20.0, maxSize / 6.0);

            const double yaw = objOps[i].centerPos.z_yaw * deg2rad;
            double cy = cos(yaw);
            double sy = sin(yaw);

            // 计算方向线的端点
            double x1 = objOps[i].centerPos.x + R * cy;
            double y1 = objOps[i].centerPos.y - R * sy;
            double x2 = objOps[i].centerPos.x - R * cy;
            double y2 = objOps[i].centerPos.y + R * sy;

            int px1 = (int)((x1 - xMin) / xScale + xSkip);
            int py1 = (int)((y1 - yMin) / yScale + ySkip);
            int px2 = (int)((x2 - xMin) / xScale + xSkip);
            int py2 = (int)((y2 - yMin) / yScale + ySkip);

            // 绘制方向线
            painter.setPen(QPen(objColor, 2));
            painter.drawLine(px1, py1, px2, py2);

            // 根据orienFlag绘制箭头
            if (objOps[i].orienFlag == 1) {
                // 绿色箭头 - 正面
                painter.setPen(QPen(QColor(0, 255, 0), 2));

                // 计算箭头端点
                double arrowLen = R / 3;
                double arrowAngle = 30 * deg2rad;
                double ca = cos(arrowAngle);
                double sa = sin(arrowAngle);

                double x3 = x1 - arrowLen * ca * cy + arrowLen * sa * sy;
                double y3 = y1 + arrowLen * ca * sy + arrowLen * sa * cy;
                double x4 = x1 - arrowLen * ca * cy - arrowLen * sa * sy;
                double y4 = y1 + arrowLen * ca * sy - arrowLen * sa * cy;

                int px3 = (int)((x3 - xMin) / xScale + xSkip);
                int py3 = (int)((y3 - yMin) / yScale + ySkip);
                int px4 = (int)((x4 - xMin) / xScale + xSkip);
                int py4 = (int)((y4 - yMin) / yScale + ySkip);

                painter.drawLine(px1, py1, px3, py3);
                painter.drawLine(px1, py1, px4, py4);
            }
            else if (objOps[i].orienFlag == 2) {
                // 蓝色箭头 - 反面
                painter.setPen(QPen(QColor(0, 0, 255), 2));

                // 计算箭头端点
                double arrowLen = R / 3;
                double arrowAngle = 30 * deg2rad;
                double ca = cos(arrowAngle);
                double sa = sin(arrowAngle);

                double x3 = x1 - arrowLen * ca * cy + arrowLen * sa * sy;
                double y3 = y1 + arrowLen * ca * sy + arrowLen * sa * cy;
                double x4 = x1 - arrowLen * ca * cy - arrowLen * sa * sy;
                double y4 = y1 + arrowLen * ca * sy - arrowLen * sa * cy;

                int px3 = (int)((x3 - xMin) / xScale + xSkip);
                int py3 = (int)((y3 - yMin) / yScale + ySkip);
                int px4 = (int)((x4 - xMin) / xScale + xSkip);
                int py4 = (int)((y4 - yMin) / yScale + ySkip);

                painter.drawLine(px1, py1, px3, py3);
                painter.drawLine(px1, py1, px4, py4);
            }

            // 绘制目标编号
            painter.setPen(QPen(Qt::white, 1));
            painter.setFont(QFont("Arial", 15, QFont::Bold));
            painter.drawText(px + 15, py - 10, QString("%1").arg(i + 1));
        }
    }
}

QImage PointCloudImageUtils::GeneratePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                                       const std::vector<std::vector<SVzNL3DPoint>>& weldResults,
                                                       int imageWidth, int imageHeight)
{
    if (scanLines.empty()) {
        return QImage();
    }

    // 计算点云范围
    double xMin, xMax, yMin, yMax;
    CalculateScanLinesRange(scanLines, xMin, xMax, yMin, yMax);

    // 检查范围是否有效
    if (xMax <= xMin || yMax <= yMin) {
        return QImage();
    }

    // 创建图像
    QImage image(imageWidth, imageHeight, QImage::Format_RGB888);
    image.fill(Qt::black);

    QPainter painter(&image);

    // 绘制点云数据
    DrawScanLinesPointCloud(painter, scanLines, xMin, xMax, yMin, yMax, imageWidth, imageHeight);

    // 绘制检测结果
    DrawLapWeldResults(painter, weldResults, xMin, xMax, yMin, yMax, imageWidth, imageHeight);

    return image;
}

void PointCloudImageUtils::CalculateScanLinesRange(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                                  double& xMin, double& xMax,
                                                  double& yMin, double& yMax)
{
    xMin = std::numeric_limits<double>::max();
    xMax = std::numeric_limits<double>::lowest();
    yMin = std::numeric_limits<double>::max();
    yMax = std::numeric_limits<double>::lowest();

    for (const auto& scanLine : scanLines) {
        for (const auto& point : scanLine) {
            if (point.pt3D.z < 1e-4) continue;

            if (point.pt3D.x < xMin) xMin = point.pt3D.x;
            if (point.pt3D.x > xMax) xMax = point.pt3D.x;
            if (point.pt3D.y < yMin) yMin = point.pt3D.y;
            if (point.pt3D.y > yMax) yMax = point.pt3D.y;
        }
    }
}

void PointCloudImageUtils::DrawScanLinesPointCloud(QPainter& painter,
                                                  const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                                  double xMin, double xMax, double yMin, double yMax,
                                                  int imageWidth, int imageHeight)
{
    double xScale = (xMax - xMin) / imageWidth;
    double yScale = (yMax - yMin) / imageHeight;

    for (const auto& scanLine : scanLines) {
        for (const auto& point : scanLine) {
            if (point.pt3D.z < 1e-4) continue;

            // 解析点索引信息
            int vType = point.nPointIdx & 0xff;
            int hType = vType >> 4;
            int objId = (point.nPointIdx >> 16) & 0xff;
            vType = vType & 0x0f;

            // 根据线特征类型确定颜色和大小
            QColor pointColor;
            int pointSize = 1;
            GetLineFeatureStyle(vType, hType, objId, pointColor, pointSize);

            // 计算图像坐标
            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(pointColor, pointSize));
                painter.drawPoint(px, py);
            }
        }
    }
}

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;

    double xScale = (xMax - xMin) / imageWidth;
    double yScale = (yMax - yMin) / 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 = (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);
            }
        }
    }
}

// Workpiece点云和角点检测结果转图像 - 将角点画成圆点
QImage PointCloudImageUtils::GeneratePointCloudRetPointImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                                       const std::vector<std::vector<SVzNL3DPoint>>& cornerPoints)
{
    if (scanLines.empty()) {
        return QImage();
    }

    // 固定图像尺寸，与其他函数保持一致
    int imgRows = 992;
    int imgCols = 1056;
    int x_skip = 16;
    int y_skip = 16;

    // 计算点云范围
    double xMin, xMax, yMin, yMax;
    CalculateScanLinesRange(scanLines, xMin, xMax, yMin, yMax);

    // 检查范围是否有效
    if (xMax <= xMin || yMax <= yMin) {
        return QImage();
    }

    // 创建图像
    QImage image(imgCols, imgRows, QImage::Format_RGB888);
    image.fill(Qt::black);

    QPainter painter(&image);

    // 计算投影比例
    double y_rows = (double)(imgRows - y_skip * 2);
    double x_cols = (double)(imgCols - x_skip * 2);
    double x_scale = (xMax - xMin) / x_cols;
    double y_scale = (yMax - yMin) / y_rows;

    // 使用统一的比例尺
    if (x_scale < y_scale)
        x_scale = y_scale;
    else
        y_scale = x_scale;

    // 绘制点云数据
    for (const auto& scanLine : scanLines) {
        for (const auto& point : scanLine) {
            if (point.pt3D.z < 1e-4) continue;

            // 解析点索引信息
            int vType = point.nPointIdx & 0xff;
            int hType = vType >> 4;
            int objId = (point.nPointIdx >> 16) & 0xff;
            vType = vType & 0x0f;

            // 根据线特征类型确定颜色和大小
            QColor pointColor;
            int pointSize = 1;
            GetLineFeatureStyle(vType, hType, objId, pointColor, pointSize);

            // 计算图像坐标
            int px = (int)((point.pt3D.x - xMin) / x_scale + x_skip);
            int py = (int)((point.pt3D.y - yMin) / y_scale + y_skip);

            if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
                painter.setPen(QPen(pointColor, pointSize));
                painter.drawPoint(px, py);
            }
        }
    }

    // 绘制角点作为圆点
    if (!cornerPoints.empty()) {
        // 定义不同组角点的颜色
        QColor cornerColors[] = {
            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(cornerColors) / sizeof(cornerColors[0]);

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

            QColor cornerColor = cornerColors[i % numColors];

            // 绘制每个角点
            for (size_t j = 0; j < cornerGroup.size(); j++) {
                const SVzNL3DPoint& corner = cornerGroup[j];

                // 计算图像坐标
                int px = (int)((corner.x - xMin) / x_scale + x_skip);
                int py = (int)((corner.y - yMin) / y_scale + y_skip);

                if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
                    // 绘制圆点标记
                    int circleSize = 10;  // 圆点直径
                    painter.setPen(QPen(cornerColor, 2));
                    painter.setBrush(QBrush(cornerColor));
                    painter.drawEllipse(px - circleSize/2, py - circleSize/2, circleSize, circleSize);

                    // 绘制角点编号，确保不超出图像边界
                    painter.setPen(QPen(Qt::white, 1));
                    QFont font("Arial", 16, QFont::Bold);
                    painter.setFont(font);

                    QString numberText = QString("%1").arg(j + 1);
                    painter.drawText(px + 8, py + 8, numberText);
                }
            }
        }
    }

    return image;
}