232 lines
8.7 KiB
C++
232 lines
8.7 KiB
C++
#include "PointCloudImageUtils.h"
|
||
#include <QPainter>
|
||
#include <cmath>
|
||
#include <algorithm>
|
||
#include <limits>
|
||
#include "VrLog.h"
|
||
|
||
#include "VrTimeUtils.h"
|
||
|
||
#ifndef PI
|
||
#define PI 3.14159265358979323846
|
||
#endif
|
||
|
||
QImage PointCloudImageUtils::GeneratePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||
const std::vector<std::vector<SVzNL3DPoint>>& detectionResults,
|
||
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\n");
|
||
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);
|
||
|
||
// 绘制检测结果（如果需要）
|
||
if (!detectionResults.empty()) {
|
||
QPainter painter(&image);
|
||
DrawLapWeldResults(painter, detectionResults, 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)
|
||
{
|
||
// 使用局部变量减少内存写入
|
||
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;
|
||
|
||
// 预计算缩放因子和偏移量
|
||
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;
|
||
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
}
|