85 lines
2.5 KiB
C++
85 lines
2.5 KiB
C++
#include "DetectionOutputConverter.h"
|
||
#include "IYTireHolePoseStatus.h"
|
||
#include <cmath>
|
||
#include <QJsonArray>
|
||
#include <QJsonObject>
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846
|
||
#endif
|
||
|
||
// 从法向量计算欧拉角（pitch/yaw 由法向量方向确定，roll 设为 0）
|
||
void DetectionOutputConverter::NormalDirToEuler(double nx, double ny, double nz,
|
||
double& roll, double& pitch, double& yaw)
|
||
{
|
||
roll = 0.0;
|
||
|
||
const double xyLen = std::sqrt(nx * nx + ny * ny);
|
||
// pitch 由 z 分量与水平面夹角确定
|
||
pitch = std::atan2(-nz, xyLen) * 180.0 / M_PI;
|
||
// yaw 由 x/y 分量确定
|
||
yaw = std::atan2(ny, nx) * 180.0 / M_PI;
|
||
}
|
||
|
||
ProtocolDetectionOutput DetectionOutputConverter::ConvertTireHoleResult(
|
||
const std::vector<TireHoleInfo>& tireHoleInfoList,
|
||
bool success,
|
||
int errorCode,
|
||
const QString& message,
|
||
int cameraIndex)
|
||
{
|
||
ProtocolDetectionOutput output;
|
||
output.success = success;
|
||
output.errorCode = errorCode;
|
||
output.message = message;
|
||
output.cameraIndex = cameraIndex;
|
||
|
||
for (const auto& hole : tireHoleInfoList) {
|
||
TireHoleDetectOutput holeOutput;
|
||
holeOutput.x = hole.centerX;
|
||
holeOutput.y = hole.centerY;
|
||
holeOutput.z = hole.centerZ;
|
||
holeOutput.holeR = hole.holeR;
|
||
NormalDirToEuler(hole.normDirX, hole.normDirY, hole.normDirZ,
|
||
holeOutput.roll, holeOutput.pitch, holeOutput.yaw);
|
||
output.tireHoleOutputs.push_back(holeOutput);
|
||
}
|
||
|
||
return output;
|
||
}
|
||
|
||
QJsonObject DetectionOutputConverter::TireHoleOutputToJson(const TireHoleDetectOutput& output)
|
||
{
|
||
QJsonObject obj;
|
||
obj["X"] = output.x;
|
||
obj["Y"] = output.y;
|
||
obj["Z"] = output.z;
|
||
obj["Roll"] = output.roll;
|
||
obj["Pitch"] = output.pitch;
|
||
obj["Yaw"] = output.yaw;
|
||
obj["HoleR"] = output.holeR;
|
||
return obj;
|
||
}
|
||
|
||
QJsonObject DetectionOutputConverter::ToJson(const ProtocolDetectionOutput& output)
|
||
{
|
||
QJsonObject response;
|
||
response["MessageType"] = "TireHoleResult";
|
||
|
||
QJsonObject data;
|
||
data["Success"] = output.success;
|
||
data["ErrorCode"] = output.errorCode;
|
||
data["Message"] = output.message;
|
||
data["CameraIndex"] = output.cameraIndex;
|
||
|
||
QJsonArray arr;
|
||
for (const auto& h : output.tireHoleOutputs) {
|
||
arr.append(TireHoleOutputToJson(h));
|
||
}
|
||
data["Count"] = static_cast<int>(output.tireHoleOutputs.size());
|
||
data["TireHoles"] = arr;
|
||
response["Data"] = data;
|
||
response["Timestamp"] = output.timestamp;
|
||
return response;
|
||
}
|