574 lines
23 KiB
C++
574 lines
23 KiB
C++
#include "DetectPresenter.h"
|
||
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <utility>
|
||
#include <vector>
|
||
|
||
#include "AapgsModelClassifier.h"
|
||
#include "SG_errCode.h"
|
||
#include "planeLocalization_Export.h"
|
||
|
||
namespace
|
||
{
|
||
struct PlaneLocalizationResult
|
||
{
|
||
SSX_planeInfo info{};
|
||
int errorCode = 0;
|
||
QString errorMessage;
|
||
};
|
||
|
||
bool IsFinitePoint(const SVzNL3DPoint& point)
|
||
{
|
||
return std::isfinite(point.x) &&
|
||
std::isfinite(point.y) &&
|
||
std::isfinite(point.z);
|
||
}
|
||
|
||
bool ConvertCloudToScanLines(
|
||
const RsCloudData& cloud,
|
||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||
QString& errorMessage)
|
||
{
|
||
scanLines.clear();
|
||
errorMessage.clear();
|
||
scanLines.reserve(cloud.size());
|
||
|
||
size_t validPointCount = 0;
|
||
for (const auto& item : cloud) {
|
||
const EVzResultDataType dataType = item.first;
|
||
const SVzLaserLineData& sourceLine = item.second;
|
||
if (sourceLine.nPointCount < 0 ||
|
||
(sourceLine.nPointCount > 0 && !sourceLine.p3DPoint)) {
|
||
errorMessage = QStringLiteral("点云包含无效扫描线");
|
||
return false;
|
||
}
|
||
if (dataType != keResultDataType_PointXYZI &&
|
||
dataType != keResultDataType_Position) {
|
||
errorMessage = QStringLiteral("停机算法不支持点云类型：%1")
|
||
.arg(static_cast<int>(dataType));
|
||
return false;
|
||
}
|
||
|
||
std::vector<SVzNL3DPosition> targetLine;
|
||
targetLine.resize(static_cast<size_t>(sourceLine.nPointCount));
|
||
if (dataType == keResultDataType_PointXYZI) {
|
||
const auto* points =
|
||
static_cast<const SVzNLPointXYZI*>(sourceLine.p3DPoint);
|
||
for (int pointIndex = 0;
|
||
pointIndex < sourceLine.nPointCount;
|
||
++pointIndex) {
|
||
SVzNL3DPosition& target =
|
||
targetLine[static_cast<size_t>(pointIndex)];
|
||
target.nPointIdx = pointIndex;
|
||
target.pt3D.x = static_cast<double>(points[pointIndex].x);
|
||
target.pt3D.y = static_cast<double>(points[pointIndex].y);
|
||
target.pt3D.z = static_cast<double>(points[pointIndex].z);
|
||
if (!IsFinitePoint(target.pt3D)) {
|
||
target.pt3D = { 0.0, 0.0, 0.0 };
|
||
} else if (std::fabs(target.pt3D.x) > 1e-6 ||
|
||
std::fabs(target.pt3D.y) > 1e-6 ||
|
||
std::fabs(target.pt3D.z) > 1e-6) {
|
||
++validPointCount;
|
||
}
|
||
}
|
||
} else {
|
||
const auto* points =
|
||
static_cast<const SVzNL3DPosition*>(sourceLine.p3DPoint);
|
||
for (int pointIndex = 0;
|
||
pointIndex < sourceLine.nPointCount;
|
||
++pointIndex) {
|
||
targetLine[static_cast<size_t>(pointIndex)] = points[pointIndex];
|
||
SVzNL3DPoint& point =
|
||
targetLine[static_cast<size_t>(pointIndex)].pt3D;
|
||
if (!IsFinitePoint(point)) {
|
||
point = { 0.0, 0.0, 0.0 };
|
||
} else if (std::fabs(point.x) > 1e-6 ||
|
||
std::fabs(point.y) > 1e-6 ||
|
||
std::fabs(point.z) > 1e-6) {
|
||
++validPointCount;
|
||
}
|
||
}
|
||
}
|
||
scanLines.push_back(std::move(targetLine));
|
||
}
|
||
|
||
if (scanLines.empty() || validPointCount == 0) {
|
||
errorMessage = QStringLiteral("点云不包含有效三维点");
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
SSX_planeParkingParam BuildParkingParam(const VrPlaneParkingParam& source)
|
||
{
|
||
SSX_planeParkingParam target{};
|
||
target.parkingPoint = { source.parkingPointX,
|
||
source.parkingPointY,
|
||
source.parkingPointZ };
|
||
target.guideLinePoint = { source.guideLinePointX,
|
||
source.guideLinePointY,
|
||
source.guideLinePointZ };
|
||
target.guidingRange = source.guidingRange;
|
||
target.parkingRange = source.parkingRange;
|
||
target.distFromNoseToWheel = source.distFromNoseToWheel;
|
||
return target;
|
||
}
|
||
|
||
SSG_planeCalibPara BuildGroundCalibrationParam(
|
||
const VrPlaneGroundCalibrationParam& source)
|
||
{
|
||
SSG_planeCalibPara target{};
|
||
for (int i = 0; i < 9; ++i) {
|
||
target.planeCalib[i] = source.planeCalib[i];
|
||
target.invRMatrix[i] = source.invRMatrix[i];
|
||
}
|
||
target.planeHeight = source.planeHeight;
|
||
return target;
|
||
}
|
||
|
||
SSG_treeGrowParam BuildTreeGrowParam(const VrPlaneTreeGrowParam& source)
|
||
{
|
||
SSG_treeGrowParam target{};
|
||
target.yDeviation_max = source.yDeviationMax;
|
||
target.zDeviation_max = source.zDeviationMax;
|
||
target.maxLineSkipNum = source.maxLineSkipNum;
|
||
target.maxSkipDistance = source.maxSkipDistance;
|
||
target.minLTypeTreeLen = source.minLTypeTreeLen;
|
||
target.minVTypeTreeLen = source.minVTypeTreeLen;
|
||
return target;
|
||
}
|
||
|
||
PlaneLocalizationResult LocalizePlane(const RsCloudData& cloud,
|
||
const VrAlgorithmParams& algorithmParams)
|
||
{
|
||
PlaneLocalizationResult result;
|
||
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
||
if (!ConvertCloudToScanLines(cloud, scanLines, result.errorMessage)) {
|
||
result.errorCode = cloud.empty() ? SG_ERR_3D_DATA_NULL
|
||
: SG_ERR_3D_DATA_INVLD;
|
||
return result;
|
||
}
|
||
|
||
std::vector<std::vector<SVzNL3DPosition>> debugData;
|
||
int algorithmError = 0;
|
||
result.info = wd_planeLocalization(
|
||
scanLines,
|
||
BuildGroundCalibrationParam(algorithmParams.groundCalibrationParam),
|
||
BuildParkingParam(algorithmParams.planeParkingParam),
|
||
BuildTreeGrowParam(algorithmParams.treeGrowParam),
|
||
debugData,
|
||
&algorithmError);
|
||
result.errorCode = algorithmError;
|
||
|
||
if (algorithmError == SX_ERR_NO_PLANE_FOUND) {
|
||
result.errorMessage = QStringLiteral("未找到有效飞机目标");
|
||
} else if (algorithmError == SX_ERR_NOSEPOINT_FAIL) {
|
||
result.errorMessage = QStringLiteral("飞机机鼻定位失败");
|
||
} else if (algorithmError == SX_ERR_ENDINE_FAIL) {
|
||
result.errorMessage = QStringLiteral("飞机主体特征提取失败");
|
||
} else if (algorithmError != 0) {
|
||
result.errorMessage = QStringLiteral("飞机定位算法错误：%1")
|
||
.arg(algorithmError);
|
||
} else if (!std::isfinite(result.info.distance) ||
|
||
!std::isfinite(result.info.deviation) ||
|
||
!std::isfinite(result.info.dirAngle_deg) ||
|
||
!IsFinitePoint(result.info.nosePoint) ||
|
||
!IsFinitePoint(result.info.axis)) {
|
||
result.errorCode = SG_ERR_3D_DATA_INVLD;
|
||
result.errorMessage = QStringLiteral("飞机定位算法返回非有限数值");
|
||
}
|
||
return result;
|
||
}
|
||
|
||
bool IsPlaneLostError(int errorCode)
|
||
{
|
||
return errorCode == SX_ERR_NO_PLANE_FOUND ||
|
||
errorCode == SX_ERR_NOSEPOINT_FAIL ||
|
||
errorCode == SX_ERR_ENDINE_FAIL ||
|
||
errorCode == SG_ERR_3D_DATA_NULL;
|
||
}
|
||
|
||
QString GuideText(ParkingGuideState state)
|
||
{
|
||
switch (state) {
|
||
case ParkingGuideState::DockingStarted: return QStringLiteral("START");
|
||
case ParkingGuideState::Capturing: return QStringLiteral("CAPTURE");
|
||
case ParkingGuideState::Tracking: return QStringLiteral("TRACKING");
|
||
case ParkingGuideState::ApproachRate: return QStringLiteral("APPROACH");
|
||
case ParkingGuideState::CenterLineAligned: return QStringLiteral("CENTER");
|
||
case ParkingGuideState::Slow: return QStringLiteral("SLOW");
|
||
case ParkingGuideState::AzimuthGuidance: return QStringLiteral("AZIMUTH");
|
||
case ParkingGuideState::StopPositionReached: return QStringLiteral("STOP");
|
||
case ParkingGuideState::DockingCompleted: return QStringLiteral("OK");
|
||
case ParkingGuideState::Overshot: return QStringLiteral("OVERSHOOT");
|
||
case ParkingGuideState::StoppedShort: return QStringLiteral("STOP SHORT");
|
||
case ParkingGuideState::SlowAircraftLost: return QStringLiteral("SLOW+LOST");
|
||
case ParkingGuideState::TooFast: return QStringLiteral("TOO FAST");
|
||
case ParkingGuideState::SystemError: return QStringLiteral("ERROR");
|
||
default: return QStringLiteral("WAIT");
|
||
}
|
||
}
|
||
|
||
void SetGuideState(ParkingSpaceGuideInfo& info, ParkingGuideState state)
|
||
{
|
||
info.guideStateCode = static_cast<int>(state);
|
||
info.guideText = GuideText(state);
|
||
}
|
||
|
||
ParkingSpaceGuideInfo MakeLastKnownInfo(
|
||
const ParkingGuideAlgorithmState& state)
|
||
{
|
||
ParkingSpaceGuideInfo info;
|
||
info.modelType = state.modelVerified ? state.modelType
|
||
: QStringLiteral("未知");
|
||
info.distance = state.lastDistance;
|
||
info.lateralOffset = state.lastDeviation;
|
||
info.angle = state.lastAngle;
|
||
info.aircraftSpeed = state.filteredApproachSpeed / 1000.0;
|
||
info.confidence = state.modelVerified ? state.modelConfidence : 0.0;
|
||
return info;
|
||
}
|
||
}
|
||
|
||
DetectPresenter::DetectPresenter()
|
||
: m_modelClassifier(std::make_unique<AapgsModelClassifier>())
|
||
{
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter() = default;
|
||
|
||
bool ModelRecognitionResult::IsVerified(double minimumConfidence) const
|
||
{
|
||
if (!std::isfinite(minimumConfidence)) {
|
||
return false;
|
||
}
|
||
|
||
const QString normalizedModel = modelType.trimmed();
|
||
const bool isUnknown = normalizedModel == QStringLiteral("未知") ||
|
||
normalizedModel.compare(QStringLiteral("unknown"), Qt::CaseInsensitive) == 0;
|
||
const double threshold = (std::max)(0.0, minimumConfidence);
|
||
return verified &&
|
||
!normalizedModel.isEmpty() &&
|
||
!isUnknown &&
|
||
std::isfinite(confidence) &&
|
||
confidence >= threshold;
|
||
}
|
||
|
||
QString DetectPresenter::GetAlgoVersion()
|
||
{
|
||
const char* planeVersion = wd_PlaneLocalizationVersion();
|
||
return QStringLiteral("planeLocalization %1 / AAPGS_model 1.0.0")
|
||
.arg(planeVersion ? QString::fromLocal8Bit(planeVersion)
|
||
: QStringLiteral("未知"));
|
||
}
|
||
|
||
int DetectPresenter::DetectAirplanePresence(const RsCloudData& cloud,
|
||
const VrAlgorithmParams& algorithmParams,
|
||
AirplanePresenceResult& result)
|
||
{
|
||
result = AirplanePresenceResult();
|
||
const PlaneLocalizationResult localization =
|
||
LocalizePlane(cloud, algorithmParams);
|
||
if (localization.errorCode == 0) {
|
||
result.state = AirplanePresenceState::Detected;
|
||
result.message = QStringLiteral("检测到飞机，距离停机点 %1 mm")
|
||
.arg(localization.info.distance, 0, 'f', 1);
|
||
return 0;
|
||
}
|
||
|
||
if (IsPlaneLostError(localization.errorCode)) {
|
||
result.state = AirplanePresenceState::NotDetected;
|
||
result.message = localization.errorMessage.trimmed().isEmpty()
|
||
? QStringLiteral("未检测到飞机")
|
||
: localization.errorMessage;
|
||
return 0;
|
||
}
|
||
|
||
result.state = AirplanePresenceState::ViewBlocked;
|
||
result.message = localization.errorMessage.trimmed().isEmpty()
|
||
? QStringLiteral("飞机检测视野或点云异常")
|
||
: localization.errorMessage;
|
||
return 0;
|
||
}
|
||
|
||
int DetectPresenter::DetectParkingSpaceGuide(const RsCloudData& cloud,
|
||
const VrAlgorithmParams& algorithmParams,
|
||
qint64 frameTimestampMs,
|
||
const ParkingGuideAlgorithmState& previousState,
|
||
DetectionResult& result,
|
||
ParkingGuideAlgorithmControl& control)
|
||
{
|
||
result = DetectionResult();
|
||
control = ParkingGuideAlgorithmControl();
|
||
control.nextState = previousState;
|
||
result.cameraIndex = 1;
|
||
result.errorCode = 0;
|
||
|
||
const VrGuideDecisionParam& guideParam = algorithmParams.guideDecisionParam;
|
||
const VrParkingProcessParam& processParam = algorithmParams.processParam;
|
||
const VrModelRecognitionParam& modelParam =
|
||
algorithmParams.modelRecognitionParam;
|
||
const PlaneLocalizationResult localization =
|
||
LocalizePlane(cloud, algorithmParams);
|
||
|
||
if (localization.errorCode != 0) {
|
||
ParkingGuideAlgorithmState& next = control.nextState;
|
||
next.lostFrameCount = (std::max)(0, previousState.lostFrameCount) + 1;
|
||
next.stoppedFrameCount = 0;
|
||
next.stoppedShortFrameCount = 0;
|
||
ParkingSpaceGuideInfo lostInfo = MakeLastKnownInfo(previousState);
|
||
const int lostThreshold = (std::max)(1, processParam.lostFrameThreshold);
|
||
const bool planeLost = IsPlaneLostError(localization.errorCode);
|
||
if (planeLost) {
|
||
if (!previousState.hasMeasurement) {
|
||
SetGuideState(lostInfo, ParkingGuideState::Waiting);
|
||
} else if (next.lostFrameCount < lostThreshold) {
|
||
ParkingGuideState transientState =
|
||
previousState.lastDistance <= processParam.slowDistance
|
||
? ParkingGuideState::Slow
|
||
: ParkingGuideStateFromCode(previousState.lastGuideStateCode);
|
||
if (transientState == ParkingGuideState::Unknown ||
|
||
transientState == ParkingGuideState::StopPositionReached ||
|
||
transientState == ParkingGuideState::DockingCompleted ||
|
||
transientState == ParkingGuideState::Overshot ||
|
||
transientState == ParkingGuideState::StoppedShort) {
|
||
transientState = ParkingGuideState::Tracking;
|
||
}
|
||
SetGuideState(lostInfo, transientState);
|
||
} else {
|
||
SetGuideState(lostInfo, ParkingGuideState::SlowAircraftLost);
|
||
lostInfo.hasException = true;
|
||
result.errorCode = localization.errorCode;
|
||
}
|
||
} else {
|
||
SetGuideState(lostInfo, ParkingGuideState::SystemError);
|
||
lostInfo.hasException = true;
|
||
result.errorCode = localization.errorCode;
|
||
}
|
||
result.parkingSpaceInfoList.push_back(lostInfo);
|
||
result.message = localization.errorMessage.trimmed().isEmpty()
|
||
? QStringLiteral("停机引导定位失败")
|
||
: localization.errorMessage;
|
||
return 0;
|
||
}
|
||
|
||
ParkingGuideAlgorithmState& next = control.nextState;
|
||
next.lostFrameCount = 0;
|
||
next.successfulFrameCount =
|
||
(std::max)(0, previousState.successfulFrameCount) + 1;
|
||
|
||
double approachSpeed = previousState.filteredApproachSpeed;
|
||
if (previousState.hasMeasurement &&
|
||
previousState.lostFrameCount == 0 &&
|
||
frameTimestampMs > previousState.lastTimestampMs) {
|
||
const double elapsedSeconds =
|
||
static_cast<double>(frameTimestampMs - previousState.lastTimestampMs) /
|
||
1000.0;
|
||
const double rawApproachSpeed =
|
||
(previousState.lastDistance - localization.info.distance) /
|
||
elapsedSeconds;
|
||
const double alpha = std::clamp(processParam.speedFilterAlpha,
|
||
0.0,
|
||
1.0);
|
||
approachSpeed = alpha * rawApproachSpeed +
|
||
(1.0 - alpha) * previousState.filteredApproachSpeed;
|
||
} else if (!previousState.hasMeasurement ||
|
||
previousState.lostFrameCount > 0) {
|
||
approachSpeed = 0.0;
|
||
}
|
||
if (!std::isfinite(approachSpeed)) {
|
||
approachSpeed = 0.0;
|
||
}
|
||
|
||
next.hasMeasurement = true;
|
||
next.lastTimestampMs = frameTimestampMs;
|
||
next.lastDistance = localization.info.distance;
|
||
next.lastDeviation = localization.info.deviation;
|
||
next.lastAngle = localization.info.dirAngle_deg;
|
||
next.lastNoseX = localization.info.nosePoint.x;
|
||
next.lastNoseY = localization.info.nosePoint.y;
|
||
next.lastNoseZ = localization.info.nosePoint.z;
|
||
next.filteredApproachSpeed = approachSpeed;
|
||
|
||
ParkingSpaceGuidePosition position;
|
||
position.x = localization.info.nosePoint.x;
|
||
position.y = localization.info.nosePoint.y;
|
||
position.z = localization.info.nosePoint.z;
|
||
position.roll = 0.0;
|
||
position.pitch = 0.0;
|
||
position.yaw = localization.info.dirAngle_deg;
|
||
result.positions.push_back(position);
|
||
|
||
ParkingSpaceGuideInfo info = MakeLastKnownInfo(next);
|
||
info.hasException = false;
|
||
|
||
const double stopTolerance =
|
||
(std::max)(0.0, processParam.stopDistanceTolerance);
|
||
const double stoppedSpeed =
|
||
(std::max)(0.0, processParam.stoppedSpeedThreshold);
|
||
const double maxApproachSpeed =
|
||
(std::max)(0.0, processParam.maxApproachSpeed);
|
||
const bool hasSpeedBaseline = previousState.hasMeasurement &&
|
||
previousState.lostFrameCount == 0;
|
||
const bool isStopped = hasSpeedBaseline &&
|
||
std::fabs(approachSpeed) <= stoppedSpeed;
|
||
const bool isAtStopPosition =
|
||
std::fabs(localization.info.distance) <= stopTolerance;
|
||
|
||
ParkingGuideState guideState = ParkingGuideState::Tracking;
|
||
if (previousState.dockingCompleted &&
|
||
(!isAtStopPosition || !isStopped)) {
|
||
next.dockingCompleted = false;
|
||
next.stopPositionReached = false;
|
||
next.completedHoldFrameCount = 0;
|
||
}
|
||
|
||
if (localization.info.distance < -stopTolerance) {
|
||
next.stopPositionReached = false;
|
||
next.dockingCompleted = false;
|
||
next.stoppedFrameCount = 0;
|
||
next.stoppedShortFrameCount = 0;
|
||
next.centerLineAligned = false;
|
||
guideState = ParkingGuideState::Overshot;
|
||
} else if (approachSpeed > maxApproachSpeed &&
|
||
maxApproachSpeed > 0.0) {
|
||
next.dockingCompleted = false;
|
||
guideState = ParkingGuideState::TooFast;
|
||
} else if (next.dockingCompleted) {
|
||
guideState = ParkingGuideState::DockingCompleted;
|
||
} else if (isAtStopPosition) {
|
||
next.stoppedShortFrameCount = 0;
|
||
if (!previousState.stopPositionReached) {
|
||
next.stopPositionReached = true;
|
||
next.completedHoldFrameCount = 0;
|
||
}
|
||
if (isStopped) {
|
||
next.stoppedFrameCount =
|
||
(std::max)(0, previousState.stoppedFrameCount) + 1;
|
||
} else {
|
||
next.stoppedFrameCount = 0;
|
||
}
|
||
next.completedHoldFrameCount =
|
||
(std::max)(0, previousState.completedHoldFrameCount) + 1;
|
||
const int stableFrames = (std::max)(1, processParam.stopStableFrames);
|
||
const int completedFrames =
|
||
(std::max)(1, processParam.completedHoldFrames);
|
||
if (next.stoppedFrameCount >= stableFrames &&
|
||
next.completedHoldFrameCount >= completedFrames) {
|
||
next.dockingCompleted = true;
|
||
guideState = ParkingGuideState::DockingCompleted;
|
||
} else {
|
||
guideState = ParkingGuideState::StopPositionReached;
|
||
}
|
||
} else {
|
||
next.stopPositionReached = false;
|
||
next.stoppedFrameCount = 0;
|
||
next.completedHoldFrameCount = 0;
|
||
|
||
const double stoppedShortDistance =
|
||
(std::max)(stopTolerance, processParam.stoppedShortMinDistance);
|
||
if (previousState.hasMeasurement &&
|
||
next.successfulFrameCount > 4 &&
|
||
isStopped &&
|
||
localization.info.distance >= stoppedShortDistance) {
|
||
next.stoppedShortFrameCount =
|
||
(std::max)(0, previousState.stoppedShortFrameCount) + 1;
|
||
} else {
|
||
next.stoppedShortFrameCount = 0;
|
||
}
|
||
|
||
if (next.stoppedShortFrameCount >=
|
||
(std::max)(1, processParam.stoppedShortStableFrames)) {
|
||
guideState = ParkingGuideState::StoppedShort;
|
||
} else if (next.successfulFrameCount == 1) {
|
||
guideState = ParkingGuideState::DockingStarted;
|
||
} else if (next.successfulFrameCount == 2) {
|
||
guideState = ParkingGuideState::Capturing;
|
||
} else if (next.successfulFrameCount == 3) {
|
||
guideState = ParkingGuideState::Tracking;
|
||
} else if (std::fabs(localization.info.deviation) >
|
||
(std::max)(0.0, guideParam.lateralTolerance) ||
|
||
std::fabs(localization.info.dirAngle_deg) >
|
||
(std::max)(0.0, guideParam.angleTolerance)) {
|
||
next.centerLineAligned = false;
|
||
guideState = ParkingGuideState::AzimuthGuidance;
|
||
} else if (!previousState.centerLineAligned) {
|
||
next.centerLineAligned = true;
|
||
guideState = ParkingGuideState::CenterLineAligned;
|
||
} else if (localization.info.distance <=
|
||
(std::max)(stopTolerance, processParam.slowDistance)) {
|
||
guideState = ParkingGuideState::Slow;
|
||
} else if (localization.info.distance <=
|
||
(std::max)(processParam.slowDistance,
|
||
processParam.approachStartDistance) &&
|
||
previousState.hasMeasurement &&
|
||
approachSpeed > 0.0) {
|
||
guideState = ParkingGuideState::ApproachRate;
|
||
} else {
|
||
guideState = ParkingGuideState::Tracking;
|
||
}
|
||
}
|
||
|
||
SetGuideState(info, guideState);
|
||
next.lastGuideStateCode = info.guideStateCode;
|
||
result.parkingSpaceInfoList.push_back(info);
|
||
result.message = QStringLiteral(
|
||
"距离 %1 mm，横向偏差 %2 mm，航向角 %3°，接近速度 %4 m/s")
|
||
.arg(info.distance, 0, 'f', 1)
|
||
.arg(info.lateralOffset, 0, 'f', 1)
|
||
.arg(info.angle, 0, 'f', 2)
|
||
.arg(info.aircraftSpeed, 0, 'f', 1);
|
||
|
||
const double modelVerifyDistance = modelParam.modelVerifyDistance;
|
||
const int maxRecognitionAttempts =
|
||
(std::max)(1, modelParam.maxRecognitionAttempts);
|
||
const bool isInModelVerifyRange = modelVerifyDistance >= 0.0 &&
|
||
info.distance >= 0.0 &&
|
||
info.distance <= modelVerifyDistance;
|
||
if (isInModelVerifyRange &&
|
||
!previousState.modelVerified &&
|
||
previousState.modelRecognitionAttempts < maxRecognitionAttempts) {
|
||
control.needModelRecognition = true;
|
||
control.recognitionContext = QByteArrayLiteral("target-index=0");
|
||
control.nextState.modelRecognitionAttempts =
|
||
(std::max)(0, previousState.modelRecognitionAttempts) + 1;
|
||
} else if (isInModelVerifyRange && !previousState.modelVerified) {
|
||
ParkingSpaceGuideInfo& failedInfo = result.parkingSpaceInfoList.back();
|
||
failedInfo.hasException = true;
|
||
failedInfo.guideStateCode =
|
||
static_cast<int>(ParkingGuideState::AircraftVerificationFailed);
|
||
failedInfo.guideText = QStringLiteral("STOP+IDFAIL");
|
||
result.message = QStringLiteral("机型二次验证失败");
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int DetectPresenter::RecognizeModel2D(const QImage& frame,
|
||
const QByteArray& recognitionContext,
|
||
ModelRecognitionResult& result)
|
||
{
|
||
Q_UNUSED(recognitionContext);
|
||
|
||
result = ModelRecognitionResult();
|
||
if (!m_modelClassifier) {
|
||
result.message = QStringLiteral("AAPGS机型识别器未初始化");
|
||
return -1;
|
||
}
|
||
|
||
AapgsModelClassifier::Classification classification;
|
||
QString errorMessage;
|
||
if (!m_modelClassifier->Classify(frame, classification, errorMessage)) {
|
||
result.message = errorMessage.trimmed().isEmpty()
|
||
? QStringLiteral("AAPGS机型识别失败")
|
||
: errorMessage.trimmed();
|
||
return -1;
|
||
}
|
||
|
||
result.modelType = classification.modelType;
|
||
result.confidence = classification.confidence;
|
||
result.verified = true;
|
||
return 0;
|
||
}
|