900 lines
37 KiB
C++
900 lines
37 KiB
C++
#include "DetectPresenter.h"
|
||
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <utility>
|
||
#include <vector>
|
||
|
||
#include "AapgsModelClassifier.h"
|
||
#include "IVrConfig.h"
|
||
#include "PersionModelDetector.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 IsPersonDetection(const PersionModelDetector::Detection& detection)
|
||
{
|
||
const QString label = detection.label.trimmed();
|
||
return label.compare(QStringLiteral("person"), Qt::CaseInsensitive) == 0 ||
|
||
(label.isEmpty() && detection.classId == 0);
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
bool IsAlignmentGuidanceState(ParkingGuideState state)
|
||
{
|
||
return state == ParkingGuideState::CenterLineAligned ||
|
||
state == ParkingGuideState::AzimuthGuidance;
|
||
}
|
||
|
||
bool IsPastAlignmentGuidanceState(ParkingGuideState state)
|
||
{
|
||
return state == ParkingGuideState::StopPositionReached ||
|
||
state == ParkingGuideState::DockingCompleted ||
|
||
state == ParkingGuideState::Overshot ||
|
||
state == ParkingGuideState::StoppedShort ||
|
||
state == ParkingGuideState::ChocksOn;
|
||
}
|
||
|
||
double IntegerMillimeters(double value)
|
||
{
|
||
if (!std::isfinite(value)) {
|
||
return 0.0;
|
||
}
|
||
return static_cast<double>(std::lround(value));
|
||
}
|
||
|
||
bool ShouldClearPublishedSpeed(int guideStateCode)
|
||
{
|
||
switch (ParkingGuideStateFromCode(guideStateCode)) {
|
||
case ParkingGuideState::StopPositionReached:
|
||
case ParkingGuideState::DockingCompleted:
|
||
case ParkingGuideState::ChocksOn:
|
||
return true;
|
||
default:
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void ApplyPublishedGuideMetrics(ParkingSpaceGuideInfo& info)
|
||
{
|
||
// 对外距离/横向偏移只报毫米整数；到位后速度按静止处理。
|
||
info.distance = IntegerMillimeters(info.distance);
|
||
info.lateralOffset = IntegerMillimeters(info.lateralOffset);
|
||
if (ShouldClearPublishedSpeed(info.guideStateCode)) {
|
||
info.aircraftSpeed = 0.0;
|
||
}
|
||
}
|
||
|
||
bool ShouldFilterMeasurementJump(ParkingGuideState previousGuideState,
|
||
double currentDistance,
|
||
double stopTolerance,
|
||
double approachStartDistance)
|
||
{
|
||
// 上一帧已在对准中线/方位引导：即使本帧跳出该距离，仍按突变忽略误检。
|
||
if (IsAlignmentGuidanceState(previousGuideState)) {
|
||
return true;
|
||
}
|
||
|
||
// 到位、越位、提前停止后不再拦截，避免锁死最终状态。
|
||
if (IsPastAlignmentGuidanceState(previousGuideState)) {
|
||
return false;
|
||
}
|
||
|
||
// 捕获/开始引导阶段仅在本帧已进入对中距离时过滤，避免误检直接跳进该过程。
|
||
return currentDistance >= 0.0 &&
|
||
currentDistance <= approachStartDistance &&
|
||
std::fabs(currentDistance) > stopTolerance;
|
||
}
|
||
|
||
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("TOOFAR");
|
||
case ParkingGuideState::StoppedShort: return QStringLiteral("STOP SHORT");
|
||
case ParkingGuideState::SlowAircraftLost: return QStringLiteral("SLOW+LOST");
|
||
case ParkingGuideState::AircraftVerificationFailed:
|
||
return QStringLiteral("STOP+IDFAIL");
|
||
case ParkingGuideState::TooFast: return QStringLiteral("STOPTOOFAST");
|
||
case ParkingGuideState::EmergencyStop: return QStringLiteral("STOP");
|
||
case ParkingGuideState::ChocksOn: return QStringLiteral("CHOCK ON");
|
||
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;
|
||
const QString selectedModelType = state.selectedModelType.trimmed();
|
||
const QString recognizedModelType = state.recognizedModelType.trimmed();
|
||
info.expectedModelType = selectedModelType;
|
||
info.modelType = recognizedModelType.isEmpty()
|
||
? QStringLiteral("未知")
|
||
: recognizedModelType;
|
||
info.modelMatched = state.modelVerified;
|
||
info.modelVerificationSupported = state.modelVerificationSupported;
|
||
info.personInChockRegion = state.personInChockRegion;
|
||
info.chocksConfirmed = state.chocksConfirmed;
|
||
info.personCount = state.personCount;
|
||
info.personConfidence = state.personConfidence;
|
||
info.distance = IntegerMillimeters(state.lastDistance);
|
||
info.lateralOffset = IntegerMillimeters(state.lastDeviation);
|
||
info.angle = state.lastAngle;
|
||
info.aircraftSpeed = state.filteredApproachSpeed / 1000.0;
|
||
if (ShouldClearPublishedSpeed(state.lastGuideStateCode)) {
|
||
info.aircraftSpeed = 0.0;
|
||
}
|
||
info.confidence = std::isfinite(state.modelConfidence)
|
||
? state.modelConfidence
|
||
: 0.0;
|
||
return info;
|
||
}
|
||
}
|
||
|
||
DetectPresenter::DetectPresenter()
|
||
: m_modelClassifier(std::make_unique<AapgsModelClassifier>())
|
||
, m_personDetector(std::make_unique<PersionModelDetector>())
|
||
{
|
||
}
|
||
|
||
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 20260804 / Persion_model 1.0.0")
|
||
.arg(planeVersion ? QString::fromLocal8Bit(planeVersion)
|
||
: QStringLiteral("未知"));
|
||
}
|
||
|
||
int DetectPresenter::CalibrateGround(
|
||
const RsCloudData& cloud,
|
||
VrPlaneGroundCalibrationParam& calibration,
|
||
QString& errorMessage)
|
||
{
|
||
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
||
if (!ConvertCloudToScanLines(cloud, scanLines, errorMessage)) {
|
||
return -1;
|
||
}
|
||
|
||
const SSG_planeCalibPara algorithmCalibration =
|
||
wd_getGroundCalibPara(scanLines);
|
||
for (int i = 0; i < 9; ++i) {
|
||
if (!std::isfinite(algorithmCalibration.planeCalib[i]) ||
|
||
!std::isfinite(algorithmCalibration.invRMatrix[i])) {
|
||
errorMessage = QStringLiteral("地面调平算法返回无效矩阵");
|
||
return -1;
|
||
}
|
||
calibration.planeCalib[i] = algorithmCalibration.planeCalib[i];
|
||
calibration.invRMatrix[i] = algorithmCalibration.invRMatrix[i];
|
||
}
|
||
if (!std::isfinite(algorithmCalibration.planeHeight)) {
|
||
errorMessage = QStringLiteral("地面调平算法返回无效地面高度");
|
||
return -1;
|
||
}
|
||
calibration.planeHeight = algorithmCalibration.planeHeight;
|
||
errorMessage.clear();
|
||
return 0;
|
||
}
|
||
|
||
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;
|
||
PlaneLocalizationResult localization =
|
||
LocalizePlane(cloud, algorithmParams);
|
||
control.planeLocalizationErrorCode = localization.errorCode;
|
||
control.planeLocalizationMessage = localization.errorMessage;
|
||
|
||
if (localization.errorCode == 0) {
|
||
// 按所选机型表中的补偿修正距离，后续判定/显示/协议都用补偿后值。
|
||
localization.info.distance -= previousState.stopDistanceCompensation;
|
||
}
|
||
|
||
if (localization.errorCode != 0) {
|
||
ParkingGuideAlgorithmState& next = control.nextState;
|
||
next.errorFrameCount =
|
||
(std::max)(0, previousState.errorFrameCount) + 1;
|
||
const bool planeLost = IsPlaneLostError(localization.errorCode);
|
||
next.lostFrameCount = planeLost
|
||
? (std::max)(0, previousState.lostFrameCount) + 1
|
||
: 0;
|
||
|
||
ParkingSpaceGuideInfo errorInfo = MakeLastKnownInfo(previousState);
|
||
const int errorThreshold =
|
||
(std::max)(1, processParam.errorFrameThreshold);
|
||
if (next.errorFrameCount < errorThreshold) {
|
||
const ParkingGuideState retainedState = previousState.hasMeasurement
|
||
? ParkingGuideStateFromCode(previousState.lastGuideStateCode)
|
||
: ParkingGuideState::Waiting;
|
||
SetGuideState(errorInfo,
|
||
retainedState == ParkingGuideState::Unknown
|
||
? ParkingGuideState::Waiting
|
||
: retainedState);
|
||
ApplyPublishedGuideMetrics(errorInfo);
|
||
result.message = previousState.hasMeasurement
|
||
? QStringLiteral(
|
||
"距离 %1 mm，横向偏差 %2 mm，航向角 %3°，接近速度 %4 m/s")
|
||
.arg(errorInfo.distance, 0, 'f', 0)
|
||
.arg(errorInfo.lateralOffset, 0, 'f', 0)
|
||
.arg(errorInfo.angle, 0, 'f', 2)
|
||
.arg(errorInfo.aircraftSpeed, 0, 'f', 1)
|
||
: QStringLiteral("等待飞机定位");
|
||
} else {
|
||
// Plane localization can briefly lose an aircraft while it is
|
||
// entering the detection area. Keep reporting WAIT for that
|
||
// expected condition instead of publishing state 14.
|
||
SetGuideState(errorInfo,
|
||
planeLost
|
||
? ParkingGuideState::Waiting
|
||
: ParkingGuideState::SystemError);
|
||
ApplyPublishedGuideMetrics(errorInfo);
|
||
errorInfo.hasException = !planeLost;
|
||
result.errorCode = planeLost ? 0 : localization.errorCode;
|
||
next.stoppedFrameCount = 0;
|
||
next.stoppedShortFrameCount = 0;
|
||
next.stopPositionReached = false;
|
||
next.dockingCompleted = false;
|
||
next.completedHoldFrameCount = 0;
|
||
next.stopStableSinceMs = -1;
|
||
result.message = localization.errorMessage.trimmed().isEmpty()
|
||
? QStringLiteral("停机引导定位失败")
|
||
: localization.errorMessage;
|
||
}
|
||
result.parkingSpaceInfoList.push_back(errorInfo);
|
||
return 0;
|
||
}
|
||
|
||
const ParkingGuideState previousGuideState =
|
||
ParkingGuideStateFromCode(previousState.lastGuideStateCode);
|
||
const double stopTolerance =
|
||
(std::max)(0.0, processParam.stopDistanceTolerance);
|
||
const double overshootDistance =
|
||
(std::max)(stopTolerance, processParam.overshootDistance);
|
||
const double slowDistance =
|
||
(std::max)(stopTolerance, processParam.slowDistance);
|
||
const double approachStartDistance =
|
||
(std::max)(slowDistance, processParam.approachStartDistance);
|
||
// 距离/横向突变过滤只用于对准中线和方位引导，捕获、到位和越位不再拦截。
|
||
if (previousState.hasMeasurement &&
|
||
ShouldFilterMeasurementJump(previousGuideState,
|
||
localization.info.distance,
|
||
stopTolerance,
|
||
approachStartDistance)) {
|
||
const double distanceChange = std::fabs(
|
||
localization.info.distance - previousState.lastDistance);
|
||
const double lateralOffsetChange = std::fabs(
|
||
localization.info.deviation - previousState.lastDeviation);
|
||
const double distanceThreshold = (std::max)(
|
||
0.0, processParam.distanceChangeThreshold);
|
||
const double lateralOffsetThreshold = (std::max)(
|
||
0.0, processParam.lateralOffsetChangeThreshold);
|
||
if (distanceChange > distanceThreshold ||
|
||
lateralOffsetChange > lateralOffsetThreshold) {
|
||
ParkingSpaceGuideInfo retainedInfo =
|
||
MakeLastKnownInfo(previousState);
|
||
const ParkingGuideState retainedState = previousGuideState;
|
||
SetGuideState(retainedInfo,
|
||
retainedState == ParkingGuideState::Unknown
|
||
? ParkingGuideState::Waiting
|
||
: retainedState);
|
||
ApplyPublishedGuideMetrics(retainedInfo);
|
||
result.parkingSpaceInfoList.push_back(retainedInfo);
|
||
result.message = QStringLiteral(
|
||
"检测数据突变已忽略：距离变化 %1 mm（阈值 %2 mm，%3），横向偏移变化 %4 mm（阈值 %5 mm，%6）")
|
||
.arg(distanceChange, 0, 'f', 1)
|
||
.arg(distanceThreshold, 0, 'f', 1)
|
||
.arg(distanceChange > distanceThreshold
|
||
? QStringLiteral("超阈值")
|
||
: QStringLiteral("未超阈值"))
|
||
.arg(lateralOffsetChange, 0, 'f', 1)
|
||
.arg(lateralOffsetThreshold, 0, 'f', 1)
|
||
.arg(lateralOffsetChange > lateralOffsetThreshold
|
||
? QStringLiteral("超阈值")
|
||
: QStringLiteral("未超阈值"));
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
ParkingGuideAlgorithmState& next = control.nextState;
|
||
next.errorFrameCount = 0;
|
||
next.lostFrameCount = 0;
|
||
next.successfulFrameCount =
|
||
(std::max)(0, previousState.successfulFrameCount) + 1;
|
||
|
||
double approachSpeed = previousState.filteredApproachSpeed;
|
||
if (previousState.hasMeasurement &&
|
||
previousState.errorFrameCount == 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.errorFrameCount > 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 captureStartDistance =
|
||
(std::max)(approachStartDistance,
|
||
processParam.captureStartDistance);
|
||
const double dockingStartDistance =
|
||
(std::max)(captureStartDistance,
|
||
processParam.dockingStartDistance);
|
||
const double stoppedSpeed =
|
||
(std::max)(0.0, processParam.stoppedSpeedThreshold);
|
||
const double maxApproachSpeed =
|
||
(std::max)(0.0, processParam.maxApproachSpeed);
|
||
const bool hasSpeedBaseline = previousState.hasMeasurement &&
|
||
previousState.errorFrameCount == 0;
|
||
const bool isStopped = hasSpeedBaseline &&
|
||
std::fabs(approachSpeed) <= stoppedSpeed;
|
||
const bool isAtStopPosition =
|
||
std::fabs(localization.info.distance) <= stopTolerance;
|
||
|
||
ParkingGuideState guideState = ParkingGuideState::Tracking;
|
||
// Overshoot must win over a previously reached stop position. The
|
||
// completion latch is only valid while the aircraft remains in the stop
|
||
// tolerance; otherwise the process must resume guidance.
|
||
if (localization.info.distance <= -overshootDistance) {
|
||
next.stopPositionReached = false;
|
||
next.dockingCompleted = false;
|
||
next.stoppedFrameCount = 0;
|
||
next.stoppedShortFrameCount = 0;
|
||
next.completedHoldFrameCount = 0;
|
||
next.stopStableSinceMs = -1;
|
||
next.centerLineAligned = false;
|
||
guideState = ParkingGuideState::Overshot;
|
||
} else if (previousState.dockingCompleted &&
|
||
isAtStopPosition &&
|
||
isStopped) {
|
||
next.dockingCompleted = true;
|
||
next.stopStableSinceMs = previousState.stopStableSinceMs;
|
||
guideState = ParkingGuideState::DockingCompleted;
|
||
} else if (isAtStopPosition) {
|
||
next.dockingCompleted = false;
|
||
next.stoppedShortFrameCount = 0;
|
||
if (!previousState.stopPositionReached) {
|
||
next.stopPositionReached = true;
|
||
next.completedHoldFrameCount = 0;
|
||
next.stopStableSinceMs = -1;
|
||
}
|
||
if (isStopped) {
|
||
next.stoppedFrameCount =
|
||
(std::max)(0, previousState.stoppedFrameCount) + 1;
|
||
} else {
|
||
next.stoppedFrameCount = 0;
|
||
next.stopStableSinceMs = -1;
|
||
}
|
||
next.completedHoldFrameCount =
|
||
(std::max)(0, previousState.completedHoldFrameCount) + 1;
|
||
const int stableFrames = (std::max)(1, processParam.stopStableFrames);
|
||
const int okHoldMs = (std::max)(0, processParam.okHoldMs);
|
||
if (next.stoppedFrameCount >= stableFrames) {
|
||
// 停稳后先保持 STOP，满 okHoldMs（默认 5 秒）再切到 OK。
|
||
if (next.stopStableSinceMs < 0) {
|
||
next.stopStableSinceMs = frameTimestampMs;
|
||
}
|
||
if (frameTimestampMs - next.stopStableSinceMs >= okHoldMs) {
|
||
next.dockingCompleted = true;
|
||
guideState = ParkingGuideState::DockingCompleted;
|
||
} else {
|
||
guideState = ParkingGuideState::StopPositionReached;
|
||
}
|
||
} else {
|
||
next.stopStableSinceMs = -1;
|
||
guideState = ParkingGuideState::StopPositionReached;
|
||
}
|
||
} else {
|
||
next.stopPositionReached = false;
|
||
next.dockingCompleted = false;
|
||
next.stoppedFrameCount = 0;
|
||
next.stoppedShortFrameCount = 0;
|
||
next.centerLineAligned = false;
|
||
next.completedHoldFrameCount = 0;
|
||
next.stopStableSinceMs = -1;
|
||
|
||
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 (localization.info.distance >= 0.0 &&
|
||
localization.info.distance <= slowDistance &&
|
||
hasSpeedBaseline &&
|
||
maxApproachSpeed > 0.0 &&
|
||
approachSpeed >= maxApproachSpeed) {
|
||
guideState = ParkingGuideState::Slow;
|
||
} else if (localization.info.distance > slowDistance &&
|
||
hasSpeedBaseline &&
|
||
maxApproachSpeed > 0.0 &&
|
||
approachSpeed >= maxApproachSpeed) {
|
||
guideState = ParkingGuideState::TooFast;
|
||
} else if (localization.info.distance >= 0.0 &&
|
||
localization.info.distance <= approachStartDistance &&
|
||
(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 (localization.info.distance >= 0.0 &&
|
||
localization.info.distance <= approachStartDistance) {
|
||
next.centerLineAligned = true;
|
||
guideState = ParkingGuideState::CenterLineAligned;
|
||
} else if (localization.info.distance <= captureStartDistance &&
|
||
localization.info.distance > approachStartDistance) {
|
||
next.centerLineAligned = false;
|
||
guideState = ParkingGuideState::Capturing;
|
||
} else if (localization.info.distance <= dockingStartDistance &&
|
||
localization.info.distance > captureStartDistance) {
|
||
next.centerLineAligned = false;
|
||
guideState = ParkingGuideState::DockingStarted;
|
||
} else if (localization.info.distance > dockingStartDistance) {
|
||
next.centerLineAligned = false;
|
||
guideState = ParkingGuideState::Waiting;
|
||
} else {
|
||
next.centerLineAligned = false;
|
||
guideState = ParkingGuideState::Tracking;
|
||
}
|
||
}
|
||
|
||
SetGuideState(info, guideState);
|
||
next.lastGuideStateCode = info.guideStateCode;
|
||
if (ShouldClearPublishedSpeed(info.guideStateCode)) {
|
||
next.filteredApproachSpeed = 0.0;
|
||
info.aircraftSpeed = 0.0;
|
||
}
|
||
ApplyPublishedGuideMetrics(info);
|
||
result.parkingSpaceInfoList.push_back(info);
|
||
result.message = QStringLiteral(
|
||
"距离 %1 mm，横向偏差 %2 mm，航向角 %3°，接近速度 %4 m/s")
|
||
.arg(info.distance, 0, 'f', 0)
|
||
.arg(info.lateralOffset, 0, 'f', 0)
|
||
.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;
|
||
const ParkingGuideState measuredState =
|
||
ParkingGuideStateFromCode(info.guideStateCode);
|
||
const bool mustPreserveMeasuredState =
|
||
measuredState == ParkingGuideState::Overshot;
|
||
const bool hasSelectedModel =
|
||
!previousState.selectedModelType.trimmed().isEmpty();
|
||
const bool requiresModelMatch =
|
||
!DetectableModelFamily(previousState.selectedModelType).isEmpty();
|
||
const bool canAttemptRecognition =
|
||
!hasSelectedModel ||
|
||
previousState.modelRecognitionAttempts < maxRecognitionAttempts;
|
||
if (isInModelVerifyRange &&
|
||
!previousState.modelRecognitionComplete &&
|
||
!previousState.modelRecognitionInProgress &&
|
||
canAttemptRecognition) {
|
||
control.needModelRecognition = true;
|
||
control.recognitionContext =
|
||
previousState.selectedModelType.trimmed().toUtf8();
|
||
} else if (isInModelVerifyRange &&
|
||
!previousState.modelRecognitionComplete &&
|
||
!previousState.modelRecognitionInProgress &&
|
||
requiresModelMatch &&
|
||
!mustPreserveMeasuredState) {
|
||
ParkingSpaceGuideInfo& failedInfo = result.parkingSpaceInfoList.back();
|
||
failedInfo.hasException = true;
|
||
failedInfo.guideStateCode =
|
||
static_cast<int>(ParkingGuideState::AircraftVerificationFailed);
|
||
failedInfo.guideText =
|
||
GuideText(ParkingGuideState::AircraftVerificationFailed);
|
||
control.nextState.lastGuideStateCode = failedInfo.guideStateCode;
|
||
result.message = QStringLiteral("机型二次验证失败");
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int DetectPresenter::RecognizeModel2D(const QImage& frame,
|
||
const QByteArray& recognitionContext,
|
||
ModelRecognitionResult& result)
|
||
{
|
||
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.recognitionComplete = true;
|
||
result.verificationSupported = true;
|
||
result.modelType = classification.modelType;
|
||
result.confidence = classification.confidence;
|
||
const QString expectedModelType =
|
||
QString::fromUtf8(recognitionContext).trimmed();
|
||
const QString recognizedModelType = result.modelType.trimmed();
|
||
if (expectedModelType.isEmpty()) {
|
||
result.message = QStringLiteral("自动识别机型：%1")
|
||
.arg(recognizedModelType);
|
||
return 0;
|
||
}
|
||
// 检测只区分 A320 / B737；选择名包含对应家族即视为匹配，例如 A320-200 NEO。
|
||
const QString expectedFamily = DetectableModelFamily(expectedModelType);
|
||
if (expectedFamily.isEmpty()) {
|
||
result.verificationSupported = false;
|
||
result.message = QStringLiteral("选择机型%1不做 A320/B737 精确校验")
|
||
.arg(expectedModelType);
|
||
return 0;
|
||
}
|
||
if (!SelectedModelMatchesDetection(expectedModelType, recognizedModelType)) {
|
||
result.message = QStringLiteral("机型验证不一致：选择%1，识别%2")
|
||
.arg(expectedModelType,
|
||
recognizedModelType.isEmpty()
|
||
? QStringLiteral("未知")
|
||
: recognizedModelType);
|
||
return 0;
|
||
}
|
||
|
||
result.verified = true;
|
||
return 0;
|
||
}
|
||
|
||
int DetectPresenter::DetectPersonInRegion2D(
|
||
const QImage& frame,
|
||
const VrPersonDetectionParam& personParam,
|
||
PersonDetectionResult& result)
|
||
{
|
||
result = PersonDetectionResult();
|
||
if (!m_personDetector || frame.isNull()) {
|
||
result.message = QStringLiteral("人员检测图像或检测器不可用");
|
||
return -1;
|
||
}
|
||
|
||
const double imageWidth = static_cast<double>(frame.width());
|
||
const double imageHeight = static_cast<double>(frame.height());
|
||
// The caller has already cropped the configured chock ROI. Detection and
|
||
// result coordinates therefore use the cropped image as their full area.
|
||
result.roi = QRectF(0.0, 0.0, imageWidth, imageHeight);
|
||
|
||
PersionModelDetector::Analysis analysis;
|
||
QString errorMessage;
|
||
if (!m_personDetector->Analyze(frame, analysis, errorMessage)) {
|
||
result.message = errorMessage.trimmed().isEmpty()
|
||
? QStringLiteral("Persion人员检测失败")
|
||
: errorMessage.trimmed();
|
||
return -1;
|
||
}
|
||
|
||
const double threshold = std::clamp(personParam.confidenceThreshold,
|
||
0.0,
|
||
1.0);
|
||
for (const PersionModelDetector::Detection& detection : analysis.detections) {
|
||
if (!IsPersonDetection(detection) ||
|
||
detection.confidence < threshold) {
|
||
continue;
|
||
}
|
||
|
||
const QRectF box = detection.boundingBox.intersected(
|
||
QRectF(0.0, 0.0, imageWidth, imageHeight));
|
||
if (box.isEmpty()) {
|
||
continue;
|
||
}
|
||
result.personBoxes.push_back(box);
|
||
|
||
++result.personCount;
|
||
result.confidence = (std::max)(result.confidence,
|
||
detection.confidence);
|
||
}
|
||
|
||
result.personInRegion = result.personCount > 0;
|
||
result.message = result.personInRegion
|
||
? QStringLiteral("轮挡区域检测到人员")
|
||
: QStringLiteral("等待人员进入轮挡区域");
|
||
return 0;
|
||
}
|