217 lines
8.7 KiB
C++
217 lines
8.7 KiB
C++
#include "DetectPresenter.h"
|
|
|
|
#include "AlgorithmParamConverter.h"
|
|
#include "PointCloudImageUtils.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 <algorithm>
|
|
|
|
SG_APISHARED_EXPORT void wd_getLineVFlaws(
|
|
std::vector<SVzNL3DPosition>& line,
|
|
int lineIndex,
|
|
SWD_valleyParam valleyParam,
|
|
std::vector<SSG_basicFeatureGap>& valleys);
|
|
|
|
SG_APISHARED_EXPORT void sg_lineGapsGrowing(
|
|
int lineIndex,
|
|
bool isLastLine,
|
|
std::vector<SSG_basicFeatureGap>& features,
|
|
std::vector<SSG_gapFeatureTree>& trees,
|
|
SSG_treeGrowParam growParam);
|
|
|
|
namespace {
|
|
|
|
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)
|
|
{
|
|
PointCloudCanvas canvas = PointCloudCanvas::Create(scanLines);
|
|
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());
|
|
}
|
|
|
|
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)
|
|
{
|
|
if (laserLines.empty()) {
|
|
detectionResult.message = QStringLiteral("扫描数据为空");
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|
}
|
|
|
|
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
|
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;
|
|
for (auto& line : scanLines) {
|
|
sx_lineDataR(line, groundCalibParam.planeCalib, -1.0);
|
|
}
|
|
|
|
std::vector<SSG_gapFeatureTree> trees;
|
|
trees.reserve(scanLines.size());
|
|
for (size_t lineIndex = 0; lineIndex < scanLines.size(); ++lineIndex) {
|
|
std::vector<SSG_basicFeatureGap> lineValleys;
|
|
wd_getLineVFlaws(scanLines[lineIndex], static_cast<int>(lineIndex),
|
|
params.valleyParam, lineValleys);
|
|
const bool isLastLine = lineIndex + 1 == scanLines.size();
|
|
sg_lineGapsGrowing(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 = 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 = BuildResultImage(scanLines, weldPaths);
|
|
SaveDebugImage(cameraIndex, debugParam, detectionResult.image);
|
|
return SUCCESS;
|
|
}
|