328 lines
13 KiB
C++
328 lines
13 KiB
C++
#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
|
||
}
|
||
}
|