473 lines
20 KiB
C++

#include "DetectPresenter.h"
#include "AlgorithmParamConverter.h"
#include "PointCloudFilter.h"
#include "PointCloudImageUtils.h"
#include "SG_baseAlgo_Export.h"
#include "SX_lapWeldDetection_Export.h"
#include "VrDateUtils.h"
#include "VrError.h"
#include "VrLog.h"
#include "VrTimeUtils.h"
#include <QColor>
#include <QCoreApplication>
#include <QDir>
#include <QFont>
#include <QPainter>
#include <QPen>
#include <QRectF>
#include <algorithm>
#include <cmath>
#include <limits>
namespace {
constexpr int kResultImageWidth = 1056;
constexpr int kResultImageHeight = 992;
constexpr int kResultImageMargin = 50;
constexpr double kImageFillRatio = 0.8;
constexpr double kMinimumImageScale = 1e-3;
int ValidPointCount(const std::vector<SVzNL3DPosition>& line)
{
return static_cast<int>(std::count_if(line.begin(), line.end(), [](const SVzNL3DPosition& point) {
return point.pt3D.z > 1e-4;
}));
}
void RemoveEmptyEdgeLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines)
{
const auto first = std::find_if(scanLines.begin(), scanLines.end(), [](const auto& line) {
return ValidPointCount(line) > 0;
});
const auto last = std::find_if(scanLines.rbegin(), scanLines.rend(), [](const auto& line) {
return ValidPointCount(line) > 0;
}).base();
if (first == scanLines.end()) {
scanLines.clear();
return;
}
std::vector<std::vector<SVzNL3DPosition>> trimmed(first, last);
scanLines.swap(trimmed);
}
QImage BuildResultImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
const std::vector<std::vector<SVzNL3DPoint>>& weldPaths)
{
// 仅过滤绘图副本,不改变焊缝检测使用的原始点云。
std::vector<std::vector<SVzNL3DPosition>> filteredScanLines;
PointCloudFilterParams filterParams;
filterParams.method = PointCloudFilterMethod::ScanLineContinuity;
filterParams.continuityThreshold = 20.0;
filterParams.minSegmentLength = 5;
PointCloudFilterStats filterStats;
const bool filterSuccess = PointCloudFilter::Filter(scanLines, filteredScanLines,
filterParams, &filterStats);
const auto& imageScanLines = filterSuccess ? filteredScanLines : scanLines;
LOG_DEBUG("WeldSeam image point filter: input=%zu output=%zu removed=%zu method=%d\n",
filterStats.inputPointCount, filterStats.outputPointCount,
filterStats.removedPointCount,
static_cast<int>(filterParams.method));
PointCloudCanvas canvas = PointCloudCanvas::Create(imageScanLines);
if (!canvas.isValid()) return QImage();
const QColor pathColor(0, 220, 255);
const QColor pointColor(255, 80, 40);
int pointIndex = 1;
for (const auto& path : weldPaths) {
for (size_t i = 1; i < path.size(); ++i) {
canvas.drawLine(path[i - 1].x, path[i - 1].y,
path[i].x, path[i].y, pathColor, 3);
}
for (const auto& point : path) {
canvas.drawPoint(point.x, point.y, pointColor, 9);
canvas.drawText(point.x, point.y, QString::number(pointIndex++),
pointColor, 13, 7, -7);
}
}
return canvas.image().copy();
}
void SaveDebugImage(int cameraIndex, const VrDebugParam& debugParam, const QImage& image)
{
if (!debugParam.enableDebug || !debugParam.saveDebugImage || image.isNull()) return;
const QString outputPath = debugParam.debugOutputPath.empty()
? QCoreApplication::applicationDirPath() + QStringLiteral("/debug")
: QString::fromStdString(debugParam.debugOutputPath);
QDir dir(outputPath);
if (!dir.exists() && !dir.mkpath(QStringLiteral("."))) {
LOG_WARNING("Failed to create WeldSeam debug image directory: %s\n",
outputPath.toUtf8().constData());
return;
}
const QString fileName = dir.filePath(
QStringLiteral("WeldSeam_%1_%2.png")
.arg(cameraIndex)
.arg(QString::fromStdString(CVrDateUtils::GetNowTime())));
if (!image.save(fileName)) {
LOG_WARNING("Failed to save WeldSeam debug image: %s\n",
fileName.toUtf8().constData());
}
}
} // namespace
DetectPresenter::DetectPresenter()
{
LOG_INFO("lapWeldDetection version: %s\n", wd_weldSeamAlgoVersion());
}
void DetectPresenter::ResetIncrementalImage()
{
m_incrementalImage = QImage();
m_imageXMin = 0.0;
m_imageYMin = 0.0;
m_imageScale = 1.0;
m_observedXMin = 0.0;
m_observedXMax = 0.0;
m_observedYMin = 0.0;
m_observedYMax = 0.0;
m_incrementalImageValid = false;
m_imageInputPointCount = 0;
m_imageOutputPointCount = 0;
}
void DetectPresenter::ResetIncrementalData()
{
std::lock_guard<std::mutex> lock(m_incrementalMutex);
m_pendingLineValleys.clear();
m_incrementalFeatureTrees.clear();
m_incrementalLineCount = 0;
ResetIncrementalImage();
}
bool DetectPresenter::HasIncrementalData() const
{
std::lock_guard<std::mutex> lock(m_incrementalMutex);
return m_incrementalLineCount > 0;
}
void DetectPresenter::AddIncrementalImageLine(
const std::vector<SVzNL3DPosition>& scanLine)
{
double lineXMin = std::numeric_limits<double>::max();
double lineXMax = std::numeric_limits<double>::lowest();
double lineYMin = std::numeric_limits<double>::max();
double lineYMax = std::numeric_limits<double>::lowest();
bool hasValidPoint = false;
for (const auto& point : scanLine) {
const SVzNL3DPoint& value = point.pt3D;
if (value.z <= 1e-4 || !std::isfinite(value.x) || !std::isfinite(value.y)) {
continue;
}
lineXMin = std::min(lineXMin, value.x);
lineXMax = std::max(lineXMax, value.x);
lineYMin = std::min(lineYMin, value.y);
lineYMax = std::max(lineYMax, value.y);
hasValidPoint = true;
}
if (!hasValidPoint) {
return;
}
if (!m_incrementalImageValid) {
m_observedXMin = lineXMin;
m_observedXMax = lineXMax;
m_observedYMin = lineYMin;
m_observedYMax = lineYMax;
} else {
m_observedXMin = std::min(m_observedXMin, lineXMin);
m_observedXMax = std::max(m_observedXMax, lineXMax);
m_observedYMin = std::min(m_observedYMin, lineYMin);
m_observedYMax = std::max(m_observedYMax, lineYMax);
}
const double drawableWidth = kResultImageWidth - kResultImageMargin * 2;
const double drawableHeight = kResultImageHeight - kResultImageMargin * 2;
const double currentXMax = m_imageXMin + drawableWidth * m_imageScale;
const double currentYMax = m_imageYMin + drawableHeight * m_imageScale;
const bool needsResize = !m_incrementalImageValid ||
m_observedXMin < m_imageXMin || m_observedXMax > currentXMax ||
m_observedYMin < m_imageYMin || m_observedYMax > currentYMax;
if (needsResize) {
const double observedWidth = std::max(m_observedXMax - m_observedXMin,
kMinimumImageScale);
const double observedHeight = std::max(m_observedYMax - m_observedYMin,
kMinimumImageScale);
const double newScale = std::max({
observedWidth / (drawableWidth * kImageFillRatio),
observedHeight / (drawableHeight * kImageFillRatio),
kMinimumImageScale
});
const double centerX = (m_observedXMin + m_observedXMax) * 0.5;
const double centerY = (m_observedYMin + m_observedYMax) * 0.5;
const double newXMin = centerX - drawableWidth * newScale * 0.5;
const double newYMin = centerY - drawableHeight * newScale * 0.5;
QImage resizedImage(kResultImageWidth, kResultImageHeight, QImage::Format_RGB888);
resizedImage.fill(Qt::black);
if (m_incrementalImageValid && !m_incrementalImage.isNull()) {
// 扫描范围扩展时只重投影固定画布,避免保留历史点云重新绘制。
const double oldWorldLeft =
m_imageXMin - kResultImageMargin * m_imageScale;
const double oldWorldTop =
m_imageYMin - kResultImageMargin * m_imageScale;
const QRectF targetRect(
(oldWorldLeft - newXMin) / newScale + kResultImageMargin,
(oldWorldTop - newYMin) / newScale + kResultImageMargin,
kResultImageWidth * m_imageScale / newScale,
kResultImageHeight * m_imageScale / newScale);
QPainter resizePainter(&resizedImage);
resizePainter.drawImage(targetRect, m_incrementalImage);
}
m_incrementalImage = std::move(resizedImage);
m_imageXMin = newXMin;
m_imageYMin = newYMin;
m_imageScale = newScale;
m_incrementalImageValid = true;
}
QPainter painter(&m_incrementalImage);
painter.setPen(QPen(QColor(150, 150, 150), 1));
for (const auto& point : scanLine) {
const SVzNL3DPoint& value = point.pt3D;
if (value.z <= 1e-4 || !std::isfinite(value.x) || !std::isfinite(value.y)) {
continue;
}
const int pixelX = static_cast<int>(
(value.x - m_imageXMin) / m_imageScale + kResultImageMargin);
const int pixelY = static_cast<int>(
(value.y - m_imageYMin) / m_imageScale + kResultImageMargin);
if (pixelX >= 0 && pixelX < kResultImageWidth &&
pixelY >= 0 && pixelY < kResultImageHeight) {
painter.drawPoint(pixelX, pixelY);
}
}
}
QImage DetectPresenter::BuildIncrementalResultImage(
const std::vector<std::vector<SVzNL3DPoint>>& weldPaths) const
{
std::lock_guard<std::mutex> lock(m_incrementalMutex);
if (!m_incrementalImageValid || m_incrementalImage.isNull()) {
return QImage();
}
QImage resultImage = m_incrementalImage.copy();
QPainter painter(&resultImage);
painter.setRenderHint(QPainter::Antialiasing);
const QColor pathColor(0, 220, 255);
const QColor pointColor(255, 80, 40);
const auto project = [this](const SVzNL3DPoint& point) {
return QPoint(
static_cast<int>((point.x - m_imageXMin) / m_imageScale +
kResultImageMargin),
static_cast<int>((point.y - m_imageYMin) / m_imageScale +
kResultImageMargin));
};
int pointIndex = 1;
for (const auto& path : weldPaths) {
painter.setPen(QPen(pathColor, 3));
for (std::size_t index = 1; index < path.size(); ++index) {
painter.drawLine(project(path[index - 1]), project(path[index]));
}
painter.setPen(QPen(pointColor, 1));
painter.setBrush(QBrush(pointColor));
painter.setFont(QFont(QStringLiteral("Arial"), 13, QFont::Bold));
for (const auto& point : path) {
const QPoint pixel = project(point);
painter.drawEllipse(pixel.x() - 4, pixel.y() - 4, 9, 9);
painter.drawText(pixel.x() + 7, pixel.y() - 7,
QString::number(pointIndex++));
}
}
LOG_DEBUG("WeldSeam streaming image filter: input=%zu output=%zu removed=%zu\n",
m_imageInputPointCount, m_imageOutputPointCount,
m_imageInputPointCount >= m_imageOutputPointCount
? m_imageInputPointCount - m_imageOutputPointCount : 0);
return resultImage;
}
bool DetectPresenter::ProcessDetectionLine(
EVzResultDataType dataType,
const SVzLaserLineData& lineData,
const VrAlgorithmParams& algorithmParams,
const SSG_planeCalibPara& groundCalibParam,
LaserDataLoader& dataLoader)
{
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> oneLine;
oneLine.emplace_back(dataType, lineData);
std::vector<std::vector<SVzNL3DPosition>> converted;
if (dataLoader.ConvertToSVzNL3DPosition(oneLine, converted) != SUCCESS ||
converted.empty() || converted.front().empty() ||
ValidPointCount(converted.front()) == 0) {
return false;
}
std::vector<SVzNL3DPosition> scanLine = std::move(converted.front());
sx_lineDataR(scanLine, groundCalibParam.planeCalib, -1.0);
const WeldSeamAlgorithmParams params = AlgorithmParamConverter::ToAlgorithmParams(algorithmParams);
std::vector<SSG_basicFeatureGap> valleys;
std::vector<SVzNL3DPosition> filteredImageLine;
PointCloudFilterParams filterParams;
filterParams.method = PointCloudFilterMethod::ScanLineContinuity;
filterParams.continuityThreshold = 20.0;
filterParams.minSegmentLength = 5;
PointCloudFilterStats filterStats;
const bool filterSuccess = PointCloudFilter::Filter(scanLine, filteredImageLine, filterParams, &filterStats);
std::lock_guard<std::mutex> lock(m_incrementalMutex);
const int lineIndex = static_cast<int>(m_incrementalLineCount);
wd_getLineVFlaws(scanLine, lineIndex, params.valleyParam, params.detectFlawType, valleys);
if (m_incrementalLineCount > 0)
{
// 当前线到达后立即归并上一条,避免逐线特征缓存随扫描长度增长。
sg_lineGapsGrowing_treeFirst(lineIndex - 1, false, m_pendingLineValleys, m_incrementalFeatureTrees, params.treeGrowParam);
}
m_pendingLineValleys = std::move(valleys);
++m_incrementalLineCount;
m_imageInputPointCount += filterStats.inputPointCount;
m_imageOutputPointCount += filterSuccess ? filterStats.outputPointCount : filterStats.inputPointCount;
AddIncrementalImageLine(filterSuccess ? filteredImageLine : scanLine);
return true;
}
QString DetectPresenter::GetAlgoVersion()
{
return QString::fromUtf8(wd_weldSeamAlgoVersion());
}
int DetectPresenter::DetectWeldSeam(
int cameraIndex,
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
const VrAlgorithmParams& algorithmParams,
const SSG_planeCalibPara& groundCalibParam,
const VrDebugParam& debugParam,
LaserDataLoader& dataLoader,
DetectionResult& detectionResult)
{
std::vector<std::vector<SVzNL3DPosition>> scanLines;
const bool useIncrementalData = HasIncrementalData();
if (useIncrementalData)
{
std::lock_guard<std::mutex> lock(m_incrementalMutex);
if (m_incrementalLineCount == 0) {
detectionResult.message = QStringLiteral("扫描数据为空");
return ERR_CODE(DEV_DATA_INVALID);
}
} else {
const int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, scanLines);
RemoveEmptyEdgeLines(scanLines);
if (convertResult != SUCCESS || scanLines.empty()) {
detectionResult.message = QStringLiteral("扫描数据转换失败");
return ERR_CODE(DEV_DATA_INVALID);
}
}
const WeldSeamAlgorithmParams params = AlgorithmParamConverter::ToAlgorithmParams(algorithmParams);
if(debugParam.enableDebug && debugParam.printDetailLog){
LOG_DEBUG("valleyParam: chkWinWidth=%.3f maxWidth=%.3f minHeight=%.3f\n",
params.valleyParam.chkWin_width, params.valleyParam.maxWidth,
params.valleyParam.minHeight);
LOG_DEBUG("weldSeamParam: refPoints=%d\n", params.weldRefPoints);
LOG_DEBUG("treeGrowParam: skipLines=%d yDev=%.3f skipDist=%.3f zDev=%.3f minL=%.3f minV=%.3f\n",
params.treeGrowParam.maxLineSkipNum,
params.treeGrowParam.yDeviation_max, params.treeGrowParam.maxSkipDistance,
params.treeGrowParam.zDeviation_max, params.treeGrowParam.minLTypeTreeLen,
params.treeGrowParam.minVTypeTreeLen);
LOG_DEBUG("planeCalib: planeHeight=%.6f\n", groundCalibParam.planeHeight);
LOG_DEBUG("planeCalib=[%.6f %.6f %.6f; %.6f %.6f %.6f; %.6f %.6f %.6f]\n",
groundCalibParam.planeCalib[0], groundCalibParam.planeCalib[1], groundCalibParam.planeCalib[2],
groundCalibParam.planeCalib[3], groundCalibParam.planeCalib[4], groundCalibParam.planeCalib[5],
groundCalibParam.planeCalib[6], groundCalibParam.planeCalib[7], groundCalibParam.planeCalib[8]);
LOG_DEBUG("invRMatrix=[%.6f %.6f %.6f; %.6f %.6f %.6f; %.6f %.6f %.6f]\n",
groundCalibParam.invRMatrix[0], groundCalibParam.invRMatrix[1], groundCalibParam.invRMatrix[2],
groundCalibParam.invRMatrix[3], groundCalibParam.invRMatrix[4], groundCalibParam.invRMatrix[5],
groundCalibParam.invRMatrix[6], groundCalibParam.invRMatrix[7], groundCalibParam.invRMatrix[8]);
}
CVrTimeUtils timer;
if (!useIncrementalData) {
for (auto& line : scanLines) {
sx_lineDataR(line, groundCalibParam.planeCalib, -1.0);
}
}
std::vector<SSG_gapFeatureTree> trees;
if (useIncrementalData) {
std::lock_guard<std::mutex> lock(m_incrementalMutex);
// STOP_SCAN 时才标记最后一线,保持与离线批处理完全一致的收尾语义。
sg_lineGapsGrowing_treeFirst(static_cast<int>(m_incrementalLineCount - 1), true,
m_pendingLineValleys, m_incrementalFeatureTrees, params.treeGrowParam);
trees.swap(m_incrementalFeatureTrees);
m_pendingLineValleys.clear();
m_incrementalLineCount = 0;
}
else
{
trees.reserve(scanLines.size());
for (std::size_t lineIndex = 0; lineIndex < scanLines.size(); ++lineIndex)
{
std::vector<SSG_basicFeatureGap> lineValleys;
wd_getLineVFlaws(scanLines[lineIndex], static_cast<int>(lineIndex),
params.valleyParam, params.detectFlawType, lineValleys);
const bool isLastLine = lineIndex + 1 == scanLines.size();
sg_lineGapsGrowing_treeFirst(static_cast<int>(lineIndex), isLastLine,
lineValleys, trees, params.treeGrowParam);
}
}
int algorithmError = 0;
std::vector<SVzNL3DPoint> weldOps;
std::vector<SVzNL3DPosition> weldRawPoints;
BQ_getWeldSeamPose(trees, params.weldRefPoints,
weldOps, weldRawPoints, &algorithmError);
const size_t weldPointCount = weldOps.size();
std::vector<std::vector<SVzNL3DPoint>> weldPaths;
if (!weldOps.empty()) weldPaths.push_back(weldOps);
LOG_INFO("BQ_getWeldSeamPose: err=%d trees=%zu points=%zu runtime=%.3fms\n",
algorithmError, trees.size(), weldPointCount,
timer.GetElapsedTimeInMilliSec());
if (debugParam.enableDebug && debugParam.printDetailLog) {
LOG_DEBUG("[Algo Thread] BQ weld result: errorCode=%d treeCount=%zu pointCount=%zu\n",
algorithmError, trees.size(), weldPointCount);
for (size_t pointIndex = 0; pointIndex < weldOps.size(); ++pointIndex) {
const auto& point = weldOps[pointIndex];
LOG_DEBUG("[Algo Thread] weldOps[%zu]: x=%.6f y=%.6f z=%.6f\n",
pointIndex, point.x, point.y, point.z);
}
}
if (algorithmError != 0 || weldPointCount == 0) {
detectionResult.success = false;
detectionResult.errorCode = algorithmError != 0 ? algorithmError : ERR_CODE(DEV_DATA_INVALID);
detectionResult.message = QStringLiteral("未检测到有效焊缝");
detectionResult.image = useIncrementalData
? BuildIncrementalResultImage(weldPaths)
: BuildResultImage(scanLines, weldPaths);
return detectionResult.errorCode;
}
detectionResult.positions.reserve(weldPointCount);
for (const auto& point : weldOps) {
detectionResult.positions.emplace_back(point.x, point.y, point.z);
}
detectionResult.success = true;
detectionResult.errorCode = 0;
detectionResult.message = QStringLiteral("焊缝检测成功");
detectionResult.image = useIncrementalData
? BuildIncrementalResultImage(weldPaths)
: BuildResultImage(scanLines, weldPaths);
SaveDebugImage(cameraIndex, debugParam, detectionResult.image);
return SUCCESS;
}