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