YangOne/Utils
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity
Utils/CloudUtils/Src/PointCloudImageUtils.cpp

1334 lines
49 KiB
C++
Raw Normal View History

init utils
2026-02-19 00:45:00 +08:00
#include "PointCloudImageUtils.h"
#include <QPainter>
#include <cmath>
#include <algorithm>
#include <limits>
#include "VrLog.h"
#ifndef PI
#define PI 3.14159265358979323846
#endif
// 线特征类型定义 - 用于点云可视化颜色区分
#ifndef LINE_FEATURE_UNDEF
#define LINE_FEATURE_UNDEF 0
#define LINE_FEATURE_L_JUMP_H2L 1
#define LINE_FEATURE_L_JUMP_L2H 2
#define LINE_FEATURE_L_SLOPE_H2L 3
#define LINE_FEATURE_L_SLOPE_L2H 4
#define LINE_FEATURE_V_SLOPE 5
#define LINE_FEATURE_LINE_ENDING_0 6
#define LINE_FEATURE_LINE_ENDING_1 7
#endif
QImage PointCloudImageUtils::GeneratePointCloudImage(SVzNL3DLaserLine* scanData,
int lineNum,
const std::vector<DetectionTargetInfo>& 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<DetectionTargetInfo>& 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的范围
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; // 返回空图像
}
// 创建图像
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;
QPainter painter(&image);
// 绘制点云
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颜色
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<DetectionTargetInfo>& 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) ? 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);
// 绘制方向线
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,
double margin)
{
if (scanLines.empty()) {
return QImage();
}
// 固定图像尺寸，与其他函数保持一致
int imgRows = 992;
int imgCols = 1056;
int x_skip = 50;
int y_skip = 50;
// 计算点云范围
double xMin, xMax, yMin, yMax;
CalculateScanLinesRange(scanLines, xMin, xMax, yMin, yMax);
// 检查范围是否有效
if (xMax <= xMin || yMax <= yMin) {
return QImage();
}
// 在3D范围上扩展边界（如果指定）
if (margin > 0.0) {
xMin -= margin;
xMax += margin;
yMin -= margin;
yMax += margin;
}
// 创建图像
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;
// 使用统一的比例尺，并计算居中偏移
double scale = std::max(x_scale, y_scale);
x_scale = scale;
y_scale = scale;
// 计算点云在图像中居中的偏移量
double cloudWidth = (xMax - xMin) / scale; // 点云在图像中的像素宽度
double cloudHeight = (yMax - yMin) / scale; // 点云在图像中的像素高度
int x_offset = x_skip + (int)((x_cols - cloudWidth) / 2); // X方向居中偏移
int y_offset = y_skip + (int)((y_rows - cloudHeight) / 2); // Y方向居中偏移
// 绘制点云数据
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_offset);
int py = (int)((point.pt3D.y - yMin) / y_scale + y_offset);
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];
// 跳过全0的点
if (fabs(corner.x) < 0.0001 && fabs(corner.y) < 0.0001 && fabs(corner.z) < 0.0001) {
continue;
}
// 计算图像坐标（使用居中偏移）
int px = (int)((corner.x - xMin) / x_scale + x_offset);
int py = (int)((corner.y - yMin) / y_scale + y_offset);
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;
}
QImage PointCloudImageUtils::GeneratePointCloudImageWithParticles(
const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
const std::vector<SimpleParticleInfo>& particles,
int cameraIndex)
{
// 创建空图像
int imageWidth = 800;
int imageHeight = 600;
QImage image(imageWidth, imageHeight, QImage::Format_RGB888);
image.fill(Qt::black);
if (scanLines.empty()) {
return image;
}
// 计算点云范围
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 (const auto& line : scanLines) {
for (const auto& pt : line) {
if (pt.pt3D.z < 1e-4) continue; // 跳过无效点
xMin = std::min(xMin, static_cast<double>(pt.pt3D.x));
xMax = std::max(xMax, static_cast<double>(pt.pt3D.x));
yMin = std::min(yMin, static_cast<double>(pt.pt3D.y));
yMax = std::max(yMax, static_cast<double>(pt.pt3D.y));
}
}
// 检查范围是否有效
if (xMax <= xMin || yMax <= yMin) {
return image;
}
// 计算缩放比例
double xScale = (xMax - xMin) / imageWidth;
double yScale = (yMax - yMin) / imageHeight;
double scale = std::max(xScale, yScale);
QPainter painter(&image);
painter.setRenderHint(QPainter::Antialiasing, true);
// 绘制点云数据（灰色）
painter.setPen(QColor(128, 128, 128));
for (const auto& line : scanLines) {
for (const auto& pt : line) {
if (pt.pt3D.z < 1e-4) continue; // 跳过无效点
int px = static_cast<int>((pt.pt3D.x - xMin) / scale);
int py = static_cast<int>((pt.pt3D.y - yMin) / scale);
if (px >= 0 && px < imageWidth && py >= 0 && py < imageHeight) {
painter.drawPoint(px, py);
}
}
}
// 绘制颗粒（用不同颜色标记）
QColor colors[] = {
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(colors) / sizeof(colors[0]);
for (size_t i = 0; i < particles.size(); i++) {
const auto& particle = particles[i];
QColor color = colors[i % numColors];
// 绘制颗粒的8个顶点和边界框
QVector<QPoint> points2D;
for (int j = 0; j < 8; j++) {
int px = static_cast<int>((particle.vertix[j].x - xMin) / scale);
int py = static_cast<int>((particle.vertix[j].y - yMin) / scale);
if (px >= 0 && px < imageWidth && py >= 0 && py < imageHeight) {
points2D.append(QPoint(px, py));
// 绘制顶点
painter.setPen(QPen(color, 3));
painter.setBrush(color);
painter.drawEllipse(QPoint(px, py), 3, 3);
}
}
// 绘制边界框（连接相邻顶点）
if (points2D.size() >= 4) {
painter.setPen(QPen(color, 1));
painter.setBrush(Qt::NoBrush);
// 假设8个顶点按照立方体的顺序排列
// 底面4个顶点：0,1,2,3 顶面4个顶点：4,5,6,7
int edges[12][2] = {
{0,1}, {1,2}, {2,3}, {3,0}, // 底面
{4,5}, {5,6}, {6,7}, {7,4}, // 顶面
{0,4}, {1,5}, {2,6}, {3,7} // 竖边
};
for (int j = 0; j < 12; j++) {
int idx1 = edges[j][0];
int idx2 = edges[j][1];
if (idx1 < points2D.size() && idx2 < points2D.size()) {
painter.drawLine(points2D[idx1], points2D[idx2]);
}
}
}
// 计算颗粒中心位置并标注序号
if (!points2D.empty()) {
QPoint center(0, 0);
for (const auto& pt : points2D) {
center.setX(center.x() + pt.x());
center.setY(center.y() + pt.y());
}
center.setX(center.x() / points2D.size());
center.setY(center.y() / points2D.size());
// 绘制序号
painter.setPen(color);
painter.setFont(QFont("Arial", 10, QFont::Bold));
painter.drawText(center, QString::number(i + 1));
}
}
return image;
}
QImage PointCloudImageUtils::GenerateWheelArchImage(
const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
const SVzNL3DPoint& wheelArchPos,
const SVzNL3DPoint& wheelUpPos,
const SVzNL3DPoint& wheelDownPos,
double archToCenterHeight,
bool hasResult)
{
// 固定图像尺寸
int imgRows = 992;
int imgCols = 1056;
int x_skip = 50;
int y_skip = 50;
// 创建图像
QImage image(imgCols, imgRows, QImage::Format_RGB888);
image.fill(Qt::black);
if (scanLines.empty()) {
return image;
}
// 计算点云范围
double xMin, xMax, yMin, yMax;
CalculateScanLinesRange(scanLines, xMin, xMax, yMin, yMax);
// 检查范围是否有效
if (xMax <= xMin || yMax <= yMin) {
return image;
}
QPainter painter(&image);
painter.setRenderHint(QPainter::Antialiasing, true);
// 计算投影比例
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 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(QColor(128, 128, 128), 1));
painter.drawPoint(px, py);
}
}
}
// 如果有检测结果，绘制轮眉检测结果
if (hasResult) {
// 绘制轮眉位置（红色大圆点）
if (fabs(wheelArchPos.x) > 0.0001 || fabs(wheelArchPos.y) > 0.0001) {
int px = (int)((wheelArchPos.x - xMin) / x_scale + x_skip);
int py = (int)((wheelArchPos.y - yMin) / y_scale + y_skip);
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
painter.setPen(QPen(QColor(255, 0, 0), 2));
painter.setBrush(QBrush(QColor(255, 0, 0)));
painter.drawEllipse(px - 8, py - 8, 16, 16);
// 标注"轮眉"
painter.setPen(QPen(Qt::white, 1));
painter.setFont(QFont("Arial", 24, QFont::Bold));
painter.drawText(px + 12, py + 5, QString::fromUtf8("轮眉"));
}
}
// 绘制上点位置（绿色圆点）
if (fabs(wheelUpPos.x) > 0.0001 || fabs(wheelUpPos.y) > 0.0001) {
int px = (int)((wheelUpPos.x - xMin) / x_scale + x_skip);
int py = (int)((wheelUpPos.y - yMin) / y_scale + y_skip);
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
painter.setPen(QPen(QColor(0, 255, 0), 2));
painter.setBrush(QBrush(QColor(0, 255, 0)));
painter.drawEllipse(px - 6, py - 6, 12, 12);
// 标注"上点"
painter.setPen(QPen(Qt::white, 1));
painter.setFont(QFont("Arial", 24, QFont::Bold));
painter.drawText(px + 10, py + 5, QString::fromUtf8("上点"));
}
}
// 绘制下点位置（蓝色圆点）
if (fabs(wheelDownPos.x) > 0.0001 || fabs(wheelDownPos.y) > 0.0001) {
int px = (int)((wheelDownPos.x - xMin) / x_scale + x_skip);
int py = (int)((wheelDownPos.y - yMin) / y_scale + y_skip);
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
painter.setPen(QPen(QColor(0, 0, 255), 2));
painter.setBrush(QBrush(QColor(0, 0, 255)));
painter.drawEllipse(px - 6, py - 6, 12, 12);
// 标注"下点"
painter.setPen(QPen(Qt::white, 1));
painter.setFont(QFont("Arial", 24, QFont::Bold));
painter.drawText(px + 10, py + 5, QString::fromUtf8("下点"));
}
}
// 绘制连线（从上点到轮眉到下点）
if ((fabs(wheelUpPos.x) > 0.0001 || fabs(wheelUpPos.y) > 0.0001) &&
(fabs(wheelArchPos.x) > 0.0001 || fabs(wheelArchPos.y) > 0.0001)) {
int px1 = (int)((wheelUpPos.x - xMin) / x_scale + x_skip);
int py1 = (int)((wheelUpPos.y - yMin) / y_scale + y_skip);
int px2 = (int)((wheelArchPos.x - xMin) / x_scale + x_skip);
int py2 = (int)((wheelArchPos.y - yMin) / y_scale + y_skip);
painter.setPen(QPen(QColor(255, 255, 0), 2, Qt::DashLine));
painter.drawLine(px1, py1, px2, py2);
}
if ((fabs(wheelArchPos.x) > 0.0001 || fabs(wheelArchPos.y) > 0.0001) &&
(fabs(wheelDownPos.x) > 0.0001 || fabs(wheelDownPos.y) > 0.0001)) {
int px1 = (int)((wheelArchPos.x - xMin) / x_scale + x_skip);
int py1 = (int)((wheelArchPos.y - yMin) / y_scale + y_skip);
int px2 = (int)((wheelDownPos.x - xMin) / x_scale + x_skip);
int py2 = (int)((wheelDownPos.y - yMin) / y_scale + y_skip);
painter.setPen(QPen(QColor(255, 255, 0), 2, Qt::DashLine));
painter.drawLine(px1, py1, px2, py2);
}
// 在图像右上角显示拱高信息
// painter.setPen(QPen(Qt::white, 1));
// painter.setFont(QFont("Arial", 14, QFont::Bold));
// QString heightText = QString::fromUtf8("拱高: %1 mm").arg(archToCenterHeight, 0, 'f', 2);
// painter.drawText(imgCols - 200, 40, heightText);
}
return image;
}
QImage PointCloudImageUtils::GenerateChannelSpaceImage(
const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
int imageWidth, int imageHeight)
{
// 创建图像
QImage image(imageWidth, imageHeight, QImage::Format_RGB888);
image.fill(Qt::black);
if (scanLines.empty()) {
return image;
}
int x_skip = 30;
int y_skip = 30;
// 计算点云范围
double xMin, xMax, yMin, yMax;
CalculateScanLinesRange(scanLines, xMin, xMax, yMin, yMax);
// 检查范围是否有效
if (xMax <= xMin || yMax <= yMin) {
return image;
}
// 计算投影比例
double y_rows = (double)(imageHeight - y_skip * 2);
double x_cols = (double)(imageWidth - x_skip * 2);
double x_scale = (xMax - xMin) / x_cols;
double y_scale = (yMax - yMin) / y_rows;
// 使用统一的比例尺
double scale = std::max(x_scale, y_scale);
// 计算点云总数和密度，动态调整点的绘制大小
int totalPoints = 0;
for (const auto& scanLine : scanLines) {
for (const auto& point : scanLine) {
if (point.pt3D.z > 1e-4) totalPoints++;
}
}
// 根据点云密度计算基础点大小（点少则画大，点多则画小）
int basePointSize = 2;
if (totalPoints < 10000) {
basePointSize = 4;
} else if (totalPoints < 50000) {
basePointSize = 3;
} else {
basePointSize = 2;
}
QPainter painter(&image);
painter.setRenderHint(QPainter::Antialiasing, false);
// 预定义8种颜色（参考 HC_chanelSpaceMeasure_test.cpp 的 objColor）
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) // 玫瑰红色
};
// 绘制点云数据 - 根据算法标记的nPointIdx着色
for (const auto& scanLine : scanLines) {
for (const auto& point : scanLine) {
if (point.pt3D.z < 1e-4) continue;
// 根据nPointIdx确定颜色和大小
// 参考 _outputRGBDScan_RGBD: nPointIdx > 0 使用objColor, 否则灰色
QColor pointColor;
int pointSize = basePointSize;
if (point.nPointIdx > 0 && point.nPointIdx <= 8) {
pointColor = objColors[point.nPointIdx - 1];
pointSize = basePointSize + 2; // 特征点更大
} else if (point.nPointIdx > 8) {
// nPointIdx > 8 使用取模方式循环颜色
pointColor = objColors[(point.nPointIdx - 1) % 8];
pointSize = basePointSize + 2;
} else {
// nPointIdx == 0 或负数，使用灰色
pointColor = QColor(180, 180, 180);
pointSize = basePointSize;
}
// 计算图像坐标
int px = (int)((point.pt3D.x - xMin) / scale + x_skip);
int py = (int)((point.pt3D.y - yMin) / scale + y_skip);
if (px >= 0 && px < imageWidth && py >= 0 && py < imageHeight) {
// 使用填充矩形绘制点，避免稀疏
painter.fillRect(px - pointSize/2, py - pointSize/2,
pointSize, pointSize, pointColor);
}
}
}
return image;
}
int PointCloudImageUtils::GenerateDepthImage(
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache,
QImage& outImage)
{
if (detectionDataCache.empty()) {
return -1;
}
// 固定图像尺寸
int imgRows = 992;
int imgCols = 1056;
int x_skip = 50;
int y_skip = 50;
// 计算点云范围
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();
double zMin = std::numeric_limits<double>::max();
double zMax = std::numeric_limits<double>::lowest();
for (const auto& dataPair : detectionDataCache) {
EVzResultDataType dataType = dataPair.first;
const SVzLaserLineData& lineData = dataPair.second;
if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue;
// 根据数据类型处理点云数据
if (dataType == keResultDataType_Position) {
SVzNL3DPosition* positions = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
for (int i = 0; i < lineData.nPointCount; i++) {
const SVzNL3DPosition& point = positions[i];
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;
if (point.pt3D.z < zMin) zMin = point.pt3D.z;
if (point.pt3D.z > zMax) zMax = point.pt3D.z;
}
} else if (dataType == keResultDataType_PointXYZRGBA) {
SVzNLPointXYZRGBA* points = static_cast<SVzNLPointXYZRGBA*>(lineData.p3DPoint);
for (int i = 0; i < lineData.nPointCount; i++) {
const SVzNLPointXYZRGBA& point = points[i];
if (point.z < 1e-4) continue;
if (point.x < xMin) xMin = point.x;
if (point.x > xMax) xMax = point.x;
if (point.y < yMin) yMin = point.y;
if (point.y > yMax) yMax = point.y;
if (point.z < zMin) zMin = point.z;
if (point.z > zMax) zMax = point.z;
}
}
}
// 检查范围是否有效
if (xMax <= xMin || yMax <= yMin) {
return -2;
}
// 创建图像
outImage = QImage(imgCols, imgRows, QImage::Format_RGB888);
outImage.fill(Qt::black);
QPainter painter(&outImage);
// 计算投影比例
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;
double zRange = zMax - zMin;
if (zRange < 1e-4) zRange = 1.0; // 防止除0
// 绘制点云数据 - 使用Z值映射颜色（深度图）
for (const auto& dataPair : detectionDataCache) {
EVzResultDataType dataType = dataPair.first;
const SVzLaserLineData& lineData = dataPair.second;
if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue;
if (dataType == keResultDataType_Position) {
SVzNL3DPosition* positions = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
for (int i = 0; i < lineData.nPointCount; i++) {
const SVzNL3DPosition& point = positions[i];
if (point.pt3D.z < 1e-4) continue;
// 计算图像坐标
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) {
// 根据Z值计算颜色（深度图效果）
double normalizedZ = (point.pt3D.z - zMin) / zRange;
normalizedZ = std::max(0.0, std::min(1.0, normalizedZ));
// 使用热力图颜色映射：蓝->青->绿->黄->红
QColor color;
if (normalizedZ < 0.25) {
int r = 0;
int g = (int)(normalizedZ * 4 * 255);
int b = 255;
color = QColor(r, g, b);
} else if (normalizedZ < 0.5) {
int r = 0;
int g = 255;
int b = (int)((0.5 - normalizedZ) * 4 * 255);
color = QColor(r, g, b);
} else if (normalizedZ < 0.75) {
int r = (int)((normalizedZ - 0.5) * 4 * 255);
int g = 255;
int b = 0;
color = QColor(r, g, b);
} else {
int r = 255;
int g = (int)((1.0 - normalizedZ) * 4 * 255);
int b = 0;
color = QColor(r, g, b);
}
painter.setPen(QPen(color, 1));
painter.drawPoint(px, py);
}
}
} else if (dataType == keResultDataType_PointXYZRGBA) {
SVzNLPointXYZRGBA* points = static_cast<SVzNLPointXYZRGBA*>(lineData.p3DPoint);
for (int i = 0; i < lineData.nPointCount; i++) {
const SVzNLPointXYZRGBA& point = points[i];
if (point.z < 1e-4) continue;
// 计算图像坐标
int px = (int)((point.x - xMin) / x_scale + x_skip);
int py = (int)((point.y - yMin) / y_scale + y_skip);
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
// 根据Z值计算颜色（深度图效果）
double normalizedZ = (point.z - zMin) / zRange;
normalizedZ = std::max(0.0, std::min(1.0, normalizedZ));
// 使用热力图颜色映射：蓝->青->绿->黄->红
QColor color;
if (normalizedZ < 0.25) {
int r = 0;
int g = (int)(normalizedZ * 4 * 255);
int b = 255;
color = QColor(r, g, b);
} else if (normalizedZ < 0.5) {
int r = 0;
int g = 255;
int b = (int)((0.5 - normalizedZ) * 4 * 255);
color = QColor(r, g, b);
} else if (normalizedZ < 0.75) {
int r = (int)((normalizedZ - 0.5) * 4 * 255);
int g = 255;
int b = 0;
color = QColor(r, g, b);
} else {
int r = 255;
int g = (int)((1.0 - normalizedZ) * 4 * 255);
int b = 0;
color = QColor(r, g, b);
}
painter.setPen(QPen(color, 1));
painter.drawPoint(px, py);
}
}
}
}
return 0;
}
Reference in New Issue Copy Permalink