462 lines
18 KiB
C++
462 lines
18 KiB
C++
#include "PointCloudImageUtils.h"
|
||
#include <QPainter>
|
||
#include <cmath>
|
||
#include <algorithm>
|
||
#include <limits>
|
||
#include <vector>
|
||
#include "VrLog.h"
|
||
|
||
#include "VrTimeUtils.h"
|
||
#include "beltTearingDetection_Export.h"
|
||
|
||
#ifndef PI
|
||
#define PI 3.14159265358979323846
|
||
#endif
|
||
|
||
namespace
|
||
{
|
||
constexpr double kPseudoColorPeriod = 50.0;
|
||
constexpr int kPseudoColorMaxCode = 65535;
|
||
|
||
bool IsValidPoint(const SVzNL3DPosition& point)
|
||
{
|
||
return std::isfinite(point.pt3D.x) &&
|
||
std::isfinite(point.pt3D.y) &&
|
||
std::isfinite(point.pt3D.z) &&
|
||
point.pt3D.z >= 1e-4;
|
||
}
|
||
|
||
int DepthToPseudoColorCode(double z)
|
||
{
|
||
const double rangeStart = std::floor(z / kPseudoColorPeriod) * kPseudoColorPeriod;
|
||
const double offset = z - rangeStart;
|
||
return (std::max)(0, (std::min)(kPseudoColorMaxCode,
|
||
static_cast<int>(std::round(offset / kPseudoColorPeriod * kPseudoColorMaxCode))));
|
||
}
|
||
|
||
const std::vector<QRgb>& GetPseudoColorLut()
|
||
{
|
||
static const std::vector<QRgb> lut = []() {
|
||
std::vector<QRgb> colors(kPseudoColorMaxCode + 1);
|
||
for (int code = 0; code <= kPseudoColorMaxCode; ++code) {
|
||
const double ratio = static_cast<double>(code) / kPseudoColorMaxCode;
|
||
// 期望的色带方向为红->品红->蓝->青->绿->黄->红。
|
||
// 反向遍历HSV色环可使相邻50 mm周期在红色处连续衔接。
|
||
const int hue = static_cast<int>(std::floor(360.0 * (1.0 - ratio))) % 360;
|
||
colors[static_cast<size_t>(code)] = QColor::fromHsv(hue, 255, 255).rgb();
|
||
}
|
||
return colors;
|
||
}();
|
||
return lut;
|
||
}
|
||
|
||
void SetDepthPixel(std::vector<float>& depth, int imageWidth, int imageHeight,
|
||
int x, int y, double z)
|
||
{
|
||
if (x < 0 || x >= imageWidth || y < 0 || y >= imageHeight) {
|
||
return;
|
||
}
|
||
|
||
float& current = depth[static_cast<size_t>(y) * imageWidth + x];
|
||
if (std::isfinite(current)) {
|
||
current = static_cast<float>((static_cast<double>(current) + z) * 0.5);
|
||
} else {
|
||
current = static_cast<float>(z);
|
||
}
|
||
}
|
||
|
||
void RasterizeDepthSegment(std::vector<float>& depth, int imageWidth, int imageHeight,
|
||
int x0, int y0, double z0, int x1, int y1, double z1,
|
||
int maxGap)
|
||
{
|
||
const int dx = std::abs(x1 - x0);
|
||
const int dy = std::abs(y1 - y0);
|
||
const int steps = (std::max)(dx, dy);
|
||
if (steps == 0) {
|
||
SetDepthPixel(depth, imageWidth, imageHeight, x0, y0, z0);
|
||
return;
|
||
}
|
||
if (steps > maxGap) {
|
||
return;
|
||
}
|
||
|
||
for (int step = 0; step <= steps; ++step) {
|
||
const double ratio = static_cast<double>(step) / steps;
|
||
const int x = static_cast<int>(std::round(x0 + (x1 - x0) * ratio));
|
||
const int y = static_cast<int>(std::round(y0 + (y1 - y0) * ratio));
|
||
const double z = z0 + (z1 - z0) * ratio;
|
||
SetDepthPixel(depth, imageWidth, imageHeight, x, y, z);
|
||
}
|
||
}
|
||
|
||
struct OrthographicProjection
|
||
{
|
||
double scale = 1.0;
|
||
double offsetX = 0.0;
|
||
double offsetY = 0.0;
|
||
};
|
||
|
||
OrthographicProjection MakeOrthographicProjection(double xMin, double xMax,
|
||
double yMin, double yMax,
|
||
int imageWidth, int imageHeight)
|
||
{
|
||
OrthographicProjection projection;
|
||
const double xRange = xMax - xMin;
|
||
const double yRange = yMax - yMin;
|
||
projection.scale = (std::min)(
|
||
static_cast<double>(imageWidth - 1) / xRange,
|
||
static_cast<double>(imageHeight - 1) / yRange);
|
||
projection.offsetX = ((imageWidth - 1) - xRange * projection.scale) * 0.5;
|
||
projection.offsetY = ((imageHeight - 1) - yRange * projection.scale) * 0.5;
|
||
return projection;
|
||
}
|
||
}
|
||
|
||
QImage PointCloudImageUtils::GeneratePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||
const std::vector<SSG_beltTearingInfo>& beltTearings,
|
||
int imageWidth, int imageHeight)
|
||
{
|
||
if (scanLines.empty() || imageWidth <= 1 || imageHeight <= 1) {
|
||
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();
|
||
}
|
||
|
||
// imageWidth/imageHeight是最大输出边界，实际尺寸按物理X/Y范围等比例生成。
|
||
const double xRange = xMax - xMin;
|
||
const double yRange = yMax - yMin;
|
||
if (xRange <= 0.0 || yRange <= 0.0) {
|
||
LOG_WARNING("Invalid point cloud range: xRange=%.2f, yRange=%.2f\n", xRange, yRange);
|
||
return QImage();
|
||
}
|
||
const double outputScale = (std::min)(
|
||
static_cast<double>(imageWidth - 1) / xRange,
|
||
static_cast<double>(imageHeight - 1) / yRange);
|
||
const int outputWidth = (std::max)(2, (std::min)(imageWidth,
|
||
static_cast<int>(std::round(xRange * outputScale)) + 1));
|
||
const int outputHeight = (std::max)(2, (std::min)(imageHeight,
|
||
static_cast<int>(std::round(yRange * outputScale)) + 1));
|
||
|
||
QImage image(outputWidth, outputHeight, QImage::Format_RGB888);
|
||
if (image.isNull()) {
|
||
LOG_ERROR("Failed to create QImage with size %dx%d\n", outputWidth, outputHeight);
|
||
return QImage();
|
||
}
|
||
|
||
image.fill(Qt::black);
|
||
|
||
// 直接绘制到图像数据，避免QPainter开销
|
||
DrawPointCloudDirect(image, scanLines, xMin, xMax, yMin, yMax, outputWidth, outputHeight);
|
||
|
||
// 绘制检测结果（如果需要）
|
||
QPainter painter(&image);
|
||
|
||
if (!beltTearings.empty()) {
|
||
DrawBeltTearingResults(painter, beltTearings, xMin, xMax, yMin, yMax, outputWidth, outputHeight);
|
||
}
|
||
|
||
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];
|
||
|
||
if (!IsValidPoint(point)) 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 || imageWidth <= 0 || imageHeight <= 0) {
|
||
LOG_WARNING("Invalid point cloud drawing range or image size\n");
|
||
return;
|
||
}
|
||
|
||
// 验证图像格式是否为RGB888
|
||
if (image.format() != QImage::Format_RGB888) {
|
||
LOG_WARNING("Image format is not RGB888, converting...\n");
|
||
image = image.convertToFormat(QImage::Format_RGB888);
|
||
}
|
||
|
||
const OrthographicProjection projection = MakeOrthographicProjection(
|
||
xMin, xMax, yMin, yMax, imageWidth, imageHeight);
|
||
const int maxConnectionGap = (std::max)(8,
|
||
(std::min)(64, (std::max)(imageWidth, imageHeight) / 16));
|
||
const float invalidDepth = std::numeric_limits<float>::quiet_NaN();
|
||
std::vector<float> depth(static_cast<size_t>(imageWidth) * imageHeight, invalidDepth);
|
||
|
||
auto toPixel = [&](const SVzNL3DPosition& point, int& px, int& py) {
|
||
px = static_cast<int>(std::round(
|
||
projection.offsetX + (point.pt3D.x - xMin) * projection.scale));
|
||
py = static_cast<int>(std::round(
|
||
projection.offsetY + (point.pt3D.y - yMin) * projection.scale));
|
||
};
|
||
|
||
for (size_t lineIndex = 0; lineIndex < scanLines.size(); ++lineIndex) {
|
||
const auto& scanLine = scanLines[lineIndex];
|
||
bool hasPrevious = false;
|
||
int previousX = 0;
|
||
int previousY = 0;
|
||
double previousZ = 0.0;
|
||
|
||
for (const auto& point : scanLine) {
|
||
if (!IsValidPoint(point)) {
|
||
hasPrevious = false;
|
||
continue;
|
||
}
|
||
|
||
int px = 0;
|
||
int py = 0;
|
||
toPixel(point, px, py);
|
||
const double z = point.pt3D.z;
|
||
SetDepthPixel(depth, imageWidth, imageHeight, px, py, z);
|
||
|
||
if (hasPrevious) {
|
||
RasterizeDepthSegment(depth, imageWidth, imageHeight,
|
||
previousX, previousY, previousZ, px, py, z, maxConnectionGap);
|
||
}
|
||
|
||
previousX = px;
|
||
previousY = py;
|
||
previousZ = z;
|
||
hasPrevious = true;
|
||
}
|
||
|
||
// 连接相邻扫描线中对应采样点，补齐 X-Y 网格间的像素空洞。
|
||
if (lineIndex > 0) {
|
||
const auto& previousLine = scanLines[lineIndex - 1];
|
||
const size_t commonCount = (std::min)(previousLine.size(), scanLine.size());
|
||
for (size_t pointIndex = 0; pointIndex < commonCount; ++pointIndex) {
|
||
const auto& previousPoint = previousLine[pointIndex];
|
||
const auto& currentPoint = scanLine[pointIndex];
|
||
if (!IsValidPoint(previousPoint) || !IsValidPoint(currentPoint)) {
|
||
continue;
|
||
}
|
||
|
||
int previousPixelX = 0;
|
||
int previousPixelY = 0;
|
||
int currentPixelX = 0;
|
||
int currentPixelY = 0;
|
||
toPixel(previousPoint, previousPixelX, previousPixelY);
|
||
toPixel(currentPoint, currentPixelX, currentPixelY);
|
||
RasterizeDepthSegment(depth, imageWidth, imageHeight,
|
||
previousPixelX, previousPixelY, previousPoint.pt3D.z,
|
||
currentPixelX, currentPixelY, currentPoint.pt3D.z,
|
||
maxConnectionGap);
|
||
}
|
||
}
|
||
}
|
||
|
||
uchar* imageData = image.bits();
|
||
if (!imageData) {
|
||
LOG_ERROR("Failed to get image data pointer\n");
|
||
return;
|
||
}
|
||
|
||
const std::vector<QRgb>& pseudoColorLut = GetPseudoColorLut();
|
||
for (int y = 0; y < imageHeight; ++y) {
|
||
for (int x = 0; x < imageWidth; ++x) {
|
||
const float z = depth[static_cast<size_t>(y) * imageWidth + x];
|
||
if (!std::isfinite(z)) {
|
||
continue;
|
||
}
|
||
|
||
const QRgb color = pseudoColorLut[static_cast<size_t>(DepthToPseudoColorCode(z))];
|
||
uchar* pixel = imageData + static_cast<size_t>(y) * image.bytesPerLine() + static_cast<size_t>(x) * 3;
|
||
pixel[0] = static_cast<uchar>(qRed(color));
|
||
pixel[1] = static_cast<uchar>(qGreen(color));
|
||
pixel[2] = static_cast<uchar>(qBlue(color));
|
||
}
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
const OrthographicProjection projection = MakeOrthographicProjection(
|
||
xMin, xMax, yMin, yMax, imageWidth, 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 = static_cast<int>(std::round(projection.offsetX +
|
||
(weldLine[j-1].x - xMin) * projection.scale));
|
||
int py1 = static_cast<int>(std::round(projection.offsetY +
|
||
(weldLine[j-1].y - yMin) * projection.scale));
|
||
int px2 = static_cast<int>(std::round(projection.offsetX +
|
||
(weldLine[j].x - xMin) * projection.scale));
|
||
int py2 = static_cast<int>(std::round(projection.offsetY +
|
||
(weldLine[j].y - yMin) * projection.scale));
|
||
|
||
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 = static_cast<int>(std::round(projection.offsetX +
|
||
(weldLine[0].x - xMin) * projection.scale));
|
||
int startY = static_cast<int>(std::round(projection.offsetY +
|
||
(weldLine[0].y - yMin) * projection.scale));
|
||
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 = static_cast<int>(std::round(projection.offsetX +
|
||
(weldLine.back().x - xMin) * projection.scale));
|
||
int endY = static_cast<int>(std::round(projection.offsetY +
|
||
(weldLine.back().y - yMin) * projection.scale));
|
||
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;
|
||
}
|
||
|
||
const OrthographicProjection projection = MakeOrthographicProjection(
|
||
xMin, xMax, yMin, yMax, imageWidth, imageHeight);
|
||
|
||
for (const auto& tearing : tearings) {
|
||
|
||
int left = static_cast<int>(std::round(projection.offsetX +
|
||
(tearing.roi.left - xMin) * projection.scale));
|
||
int right = static_cast<int>(std::round(projection.offsetX +
|
||
(tearing.roi.right - xMin) * projection.scale));
|
||
int top = static_cast<int>(std::round(projection.offsetY +
|
||
(tearing.roi.top - yMin) * projection.scale));
|
||
int bottom = static_cast<int>(std::round(projection.offsetY +
|
||
(tearing.roi.bottom - yMin) * projection.scale));
|
||
if (left > right) std::swap(left, right);
|
||
if (top > bottom) std::swap(top, bottom);
|
||
left = (std::max)(0, (std::min)(imageWidth - 1, left));
|
||
right = (std::max)(0, (std::min)(imageWidth - 1, right));
|
||
top = (std::max)(0, (std::min)(imageHeight - 1, top));
|
||
bottom = (std::max)(0, (std::min)(imageHeight - 1, bottom));
|
||
if (right > left && bottom > top) {
|
||
const QRect roiRect(left, top, right - left, bottom - top);
|
||
painter.setBrush(Qt::NoBrush);
|
||
painter.setPen(QPen(Qt::green, 2));
|
||
painter.drawRect(roiRect.adjusted(-1, -1, 1, 1));
|
||
painter.setPen(QPen(Qt::red, 2));
|
||
painter.drawRect(roiRect);
|
||
}
|
||
}
|
||
}
|