470 lines
22 KiB
C++
470 lines
22 KiB
C++
#include "VrWheelMeasureConfig.h"
|
||
#include "IVrWheelMeasureConfig.h"
|
||
#include <algorithm>
|
||
#include <sstream>
|
||
#include "VrLog.h"
|
||
#include <QFile>
|
||
#include <QTextStream>
|
||
#include <QString>
|
||
#include <QTextCodec>
|
||
#include <QXmlStreamReader>
|
||
#include <QXmlStreamWriter>
|
||
|
||
VrWheelMeasureConfig::VrWheelMeasureConfig()
|
||
: m_notify(nullptr)
|
||
{
|
||
}
|
||
|
||
VrWheelMeasureConfig::~VrWheelMeasureConfig()
|
||
{
|
||
}
|
||
|
||
// 静态工厂方法
|
||
bool IVrWheelMeasureConfig::CreateInstance(IVrWheelMeasureConfig** ppVrConfig)
|
||
{
|
||
if (!ppVrConfig) {
|
||
return false;
|
||
}
|
||
|
||
*ppVrConfig = new VrWheelMeasureConfig();
|
||
return true;
|
||
}
|
||
|
||
WheelMeasureConfigResult VrWheelMeasureConfig::LoadConfig(const std::string& filePath)
|
||
{
|
||
WheelMeasureConfigResult result;
|
||
|
||
// 使用QString处理可能包含中文的路径
|
||
QString qFilePath = QString::fromStdString(filePath);
|
||
QFile file(qFilePath);
|
||
|
||
// 检查文件是否存在并可读
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
LOG_DEBUG("Failed to open file: %s\n", filePath.c_str());
|
||
return result;
|
||
}
|
||
|
||
// 使用QXmlStreamReader解析XML内容
|
||
QXmlStreamReader xml(&file);
|
||
|
||
// 读取到根元素
|
||
if (xml.readNextStartElement()) {
|
||
if (xml.name() != "WheelMeasureConfig") {
|
||
xml.raiseError(QObject::tr("Not a WheelMeasureConfig file"));
|
||
}
|
||
} else {
|
||
xml.raiseError(QObject::tr("Failed to read root element"));
|
||
}
|
||
|
||
// 解析XML内容
|
||
while (!xml.atEnd() && !xml.hasError()) {
|
||
xml.readNext();
|
||
|
||
// 解析相机配置
|
||
if (xml.isStartElement() && xml.name() == "Cameras") {
|
||
while (xml.readNextStartElement()) {
|
||
if (xml.name() == "Camera") {
|
||
WheelCameraParam camera;
|
||
camera.cameraIndex = xml.attributes().value("index").toInt();
|
||
camera.name = xml.attributes().value("name").toString().toStdString();
|
||
camera.cameraIP = xml.attributes().value("ip").toString().toStdString();
|
||
camera.enabled = xml.attributes().value("enabled").toInt() != 0;
|
||
result.cameras.push_back(camera);
|
||
xml.skipCurrentElement();
|
||
}
|
||
}
|
||
}
|
||
|
||
// 解析相机调平参数
|
||
else if (xml.isStartElement() && xml.name() == "PlaneCalibParams") {
|
||
while (xml.readNextStartElement()) {
|
||
if (xml.name() == "CameraCalib") {
|
||
WheelCameraPlaneCalibParam calibParam;
|
||
calibParam.cameraIndex = xml.attributes().value("index").toInt();
|
||
calibParam.cameraName = xml.attributes().value("name").toString().toStdString();
|
||
calibParam.planeHeight = xml.attributes().value("planeHeight").toDouble();
|
||
calibParam.isCalibrated = xml.attributes().value("isCalibrated").toInt() != 0;
|
||
// 读取误差补偿参数，默认值为-5.0
|
||
if (xml.attributes().hasAttribute("errorCompensation")) {
|
||
calibParam.errorCompensation = xml.attributes().value("errorCompensation").toDouble();
|
||
} else {
|
||
calibParam.errorCompensation = -5.0;
|
||
}
|
||
|
||
// 读取修正系数，默认值为1.0
|
||
if (xml.attributes().hasAttribute("correctionFactor")) {
|
||
calibParam.correctionFactor = xml.attributes().value("correctionFactor").toDouble();
|
||
} else {
|
||
calibParam.correctionFactor = 1.0;
|
||
}
|
||
|
||
// 读取轮胎存在检测的3D ROI范围
|
||
if (xml.attributes().hasAttribute("wheelRoi3d_xMin")) {
|
||
calibParam.wheelRoi3d_xMin = xml.attributes().value("wheelRoi3d_xMin").toDouble();
|
||
calibParam.wheelRoi3d_xMax = xml.attributes().value("wheelRoi3d_xMax").toDouble();
|
||
calibParam.wheelRoi3d_yMin = xml.attributes().value("wheelRoi3d_yMin").toDouble();
|
||
calibParam.wheelRoi3d_yMax = xml.attributes().value("wheelRoi3d_yMax").toDouble();
|
||
calibParam.wheelRoi3d_zMin = xml.attributes().value("wheelRoi3d_zMin").toDouble();
|
||
calibParam.wheelRoi3d_zMax = xml.attributes().value("wheelRoi3d_zMax").toDouble();
|
||
} else {
|
||
// 默认值
|
||
calibParam.wheelRoi3d_xMin = -1000.0;
|
||
calibParam.wheelRoi3d_xMax = 1000.0;
|
||
calibParam.wheelRoi3d_yMin = -1000.0;
|
||
calibParam.wheelRoi3d_yMax = 1000.0;
|
||
calibParam.wheelRoi3d_zMin = -1000.0;
|
||
calibParam.wheelRoi3d_zMax = 1000.0;
|
||
}
|
||
|
||
// 读取planeCalib矩阵
|
||
QString planeCalibStr = xml.attributes().value("planeCalib").toString();
|
||
QStringList planeCalibList = planeCalibStr.split(",");
|
||
for (int i = 0; i < 9 && i < planeCalibList.size(); ++i) {
|
||
calibParam.planeCalib[i] = planeCalibList[i].toDouble();
|
||
}
|
||
|
||
// 读取invRMatrix矩阵
|
||
QString invRMatrixStr = xml.attributes().value("invRMatrix").toString();
|
||
QStringList invRMatrixList = invRMatrixStr.split(",");
|
||
for (int i = 0; i < 9 && i < invRMatrixList.size(); ++i) {
|
||
calibParam.invRMatrix[i] = invRMatrixList[i].toDouble();
|
||
}
|
||
|
||
result.planeCalibParams.push_back(calibParam);
|
||
xml.skipCurrentElement();
|
||
}
|
||
}
|
||
}
|
||
|
||
// 解析算法参数
|
||
else if (xml.isStartElement() && xml.name() == "AlgorithmParams") {
|
||
while (xml.readNextStartElement()) {
|
||
// 角点参数
|
||
if (xml.name() == "CornerParam") {
|
||
result.algorithmParams.cornerParam.minEndingGap =
|
||
xml.attributes().value("minEndingGap").toDouble();
|
||
result.algorithmParams.cornerParam.minEndingGap_z =
|
||
xml.attributes().value("minEndingGap_z").toDouble();
|
||
result.algorithmParams.cornerParam.scale =
|
||
xml.attributes().value("scale").toDouble();
|
||
result.algorithmParams.cornerParam.cornerTh =
|
||
xml.attributes().value("cornerTh").toDouble();
|
||
result.algorithmParams.cornerParam.jumpCornerTh_1 =
|
||
xml.attributes().value("jumpCornerTh_1").toDouble();
|
||
result.algorithmParams.cornerParam.jumpCornerTh_2 =
|
||
xml.attributes().value("jumpCornerTh_2").toDouble();
|
||
|
||
// 设置默认值
|
||
if (result.algorithmParams.cornerParam.minEndingGap == 0.0) {
|
||
result.algorithmParams.cornerParam.minEndingGap = 3.0;
|
||
}
|
||
if (result.algorithmParams.cornerParam.minEndingGap_z == 0.0) {
|
||
result.algorithmParams.cornerParam.minEndingGap_z = 5.0;
|
||
}
|
||
if (result.algorithmParams.cornerParam.scale == 0.0) {
|
||
result.algorithmParams.cornerParam.scale = 10.0;
|
||
}
|
||
if (result.algorithmParams.cornerParam.cornerTh == 0.0) {
|
||
result.algorithmParams.cornerParam.cornerTh = 130.0;
|
||
}
|
||
if (result.algorithmParams.cornerParam.jumpCornerTh_1 == 0.0) {
|
||
result.algorithmParams.cornerParam.jumpCornerTh_1 = 5.0;
|
||
}
|
||
if (result.algorithmParams.cornerParam.jumpCornerTh_2 == 0.0) {
|
||
result.algorithmParams.cornerParam.jumpCornerTh_2 = 2.0;
|
||
}
|
||
|
||
xml.skipCurrentElement();
|
||
}
|
||
// 线段参数
|
||
else if (xml.name() == "LineSegParam") {
|
||
result.algorithmParams.lineSegParam.segGapTh_y =
|
||
xml.attributes().value("segGapTh_y").toDouble();
|
||
result.algorithmParams.lineSegParam.segGapTh_z =
|
||
xml.attributes().value("segGapTh_z").toDouble();
|
||
result.algorithmParams.lineSegParam.maxDist =
|
||
xml.attributes().value("maxDist").toDouble();
|
||
|
||
// 设置默认值
|
||
if (result.algorithmParams.lineSegParam.segGapTh_y == 0.0) {
|
||
result.algorithmParams.lineSegParam.segGapTh_y = 5.0;
|
||
}
|
||
if (result.algorithmParams.lineSegParam.segGapTh_z == 0.0) {
|
||
result.algorithmParams.lineSegParam.segGapTh_z = 10.0;
|
||
}
|
||
if (result.algorithmParams.lineSegParam.maxDist == 0.0) {
|
||
result.algorithmParams.lineSegParam.maxDist = 50.0;
|
||
}
|
||
|
||
xml.skipCurrentElement();
|
||
}
|
||
// 离群点过滤参数
|
||
else if (xml.name() == "OutlierFilterParam") {
|
||
result.algorithmParams.filterParam.continuityTh =
|
||
xml.attributes().value("continuityTh").toDouble();
|
||
result.algorithmParams.filterParam.outlierTh =
|
||
xml.attributes().value("outlierTh").toDouble();
|
||
|
||
// 设置默认值
|
||
if (result.algorithmParams.filterParam.continuityTh == 0.0) {
|
||
result.algorithmParams.filterParam.continuityTh = 5.0;
|
||
}
|
||
if (result.algorithmParams.filterParam.outlierTh == 0.0) {
|
||
result.algorithmParams.filterParam.outlierTh = 3.0;
|
||
}
|
||
|
||
xml.skipCurrentElement();
|
||
}
|
||
// 树生长参数
|
||
else if (xml.name() == "TreeGrowParam") {
|
||
result.algorithmParams.growParam.yDeviation_max =
|
||
xml.attributes().value("yDeviation_max").toDouble();
|
||
result.algorithmParams.growParam.zDeviation_max =
|
||
xml.attributes().value("zDeviation_max").toDouble();
|
||
result.algorithmParams.growParam.maxLineSkipNum =
|
||
xml.attributes().value("maxLineSkipNum").toInt();
|
||
result.algorithmParams.growParam.maxSkipDistance =
|
||
xml.attributes().value("maxSkipDistance").toDouble();
|
||
result.algorithmParams.growParam.minLTypeTreeLen =
|
||
xml.attributes().value("minLTypeTreeLen").toDouble();
|
||
result.algorithmParams.growParam.minVTypeTreeLen =
|
||
xml.attributes().value("minVTypeTreeLen").toDouble();
|
||
|
||
// 设置默认值
|
||
if (result.algorithmParams.growParam.yDeviation_max == 0.0) {
|
||
result.algorithmParams.growParam.yDeviation_max = 20.0;
|
||
}
|
||
if (result.algorithmParams.growParam.zDeviation_max == 0.0) {
|
||
result.algorithmParams.growParam.zDeviation_max = 30.0;
|
||
}
|
||
if (result.algorithmParams.growParam.maxLineSkipNum == 0) {
|
||
result.algorithmParams.growParam.maxLineSkipNum = 5;
|
||
}
|
||
if (result.algorithmParams.growParam.minLTypeTreeLen == 0.0) {
|
||
result.algorithmParams.growParam.minLTypeTreeLen = 10.0;
|
||
}
|
||
if (result.algorithmParams.growParam.minVTypeTreeLen == 0.0) {
|
||
result.algorithmParams.growParam.minVTypeTreeLen = 10.0;
|
||
}
|
||
|
||
xml.skipCurrentElement();
|
||
}
|
||
else {
|
||
xml.skipCurrentElement();
|
||
}
|
||
}
|
||
}
|
||
|
||
// 解析调试参数
|
||
else if (xml.isStartElement() && xml.name() == "DebugParam") {
|
||
result.debugParam.enableDebug = xml.attributes().value("enableDebug").toInt();
|
||
result.debugParam.savePointCloud = xml.attributes().value("savePointCloud").toInt();
|
||
result.debugParam.saveDebugImage = xml.attributes().value("saveDebugImage").toInt();
|
||
result.debugParam.printDetailLog = xml.attributes().value("printDetailLog").toInt();
|
||
result.debugParam.debugOutputPath = xml.attributes().value("debugOutputPath").toString().toStdString();
|
||
xml.skipCurrentElement();
|
||
}
|
||
|
||
// 解析服务端配置
|
||
else if (xml.isStartElement() && xml.name() == "LocalServerConfig") {
|
||
while (xml.readNextStartElement()) {
|
||
if (xml.name() == "ServerPort") {
|
||
result.serverPort = xml.attributes().value("port").toInt();
|
||
xml.skipCurrentElement();
|
||
} else if (xml.name() == "TcpPort") {
|
||
result.tcpPort = xml.attributes().value("port").toInt();
|
||
if (result.tcpPort == 0) {
|
||
result.tcpPort = 5800; // 默认值
|
||
}
|
||
xml.skipCurrentElement();
|
||
} else {
|
||
xml.skipCurrentElement();
|
||
}
|
||
}
|
||
}
|
||
|
||
// 解析服务器列表
|
||
else if (xml.isStartElement() && xml.name() == "Servers") {
|
||
while (xml.readNextStartElement()) {
|
||
if (xml.name() == "Server") {
|
||
WheelServerInfo server;
|
||
server.name = xml.attributes().value("name").toString().toStdString();
|
||
server.ip = xml.attributes().value("ip").toString().toStdString();
|
||
server.port = xml.attributes().value("port").toInt();
|
||
if (server.port == 0) {
|
||
server.port = 5800; // 默认端口
|
||
}
|
||
result.servers.push_back(server);
|
||
xml.skipCurrentElement();
|
||
} else {
|
||
xml.skipCurrentElement();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
file.close();
|
||
|
||
// 检查解析错误
|
||
if (xml.hasError()) {
|
||
LOG_ERROR("XML parsing error: %s\n", xml.errorString().toStdString().c_str());
|
||
return WheelMeasureConfigResult(); // 返回空结果
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
bool VrWheelMeasureConfig::SaveConfig(const std::string& filePath, WheelMeasureConfigResult& configResult)
|
||
{
|
||
// 使用QString处理可能包含中文的路径
|
||
QString qFilePath = QString::fromStdString(filePath);
|
||
QFile file(qFilePath);
|
||
|
||
// 打开文件进行写入
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
|
||
LOG_DEBUG("Failed to open file for writing: %s\n", filePath.c_str());
|
||
return false;
|
||
}
|
||
|
||
// 使用QXmlStreamWriter写入XML内容
|
||
QXmlStreamWriter xml(&file);
|
||
xml.setAutoFormatting(true);
|
||
xml.setCodec("UTF-8");
|
||
xml.writeStartDocument();
|
||
xml.writeStartElement("WheelMeasureConfig");
|
||
|
||
// 保存相机配置
|
||
xml.writeStartElement("Cameras");
|
||
for (const auto& camera : configResult.cameras) {
|
||
xml.writeStartElement("Camera");
|
||
xml.writeAttribute("index", QString::number(camera.cameraIndex));
|
||
xml.writeAttribute("name", QString::fromStdString(camera.name));
|
||
xml.writeAttribute("ip", QString::fromStdString(camera.cameraIP));
|
||
xml.writeAttribute("enabled", QString::number(camera.enabled ? 1 : 0));
|
||
xml.writeEndElement(); // Camera
|
||
}
|
||
xml.writeEndElement(); // Cameras
|
||
|
||
// 保存相机调平参数
|
||
xml.writeStartElement("PlaneCalibParams");
|
||
for (const auto& calibParam : configResult.planeCalibParams) {
|
||
xml.writeStartElement("CameraCalib");
|
||
xml.writeAttribute("index", QString::number(calibParam.cameraIndex));
|
||
xml.writeAttribute("name", QString::fromStdString(calibParam.cameraName));
|
||
xml.writeAttribute("planeHeight", QString::number(calibParam.planeHeight, 'f', 6));
|
||
xml.writeAttribute("isCalibrated", QString::number(calibParam.isCalibrated ? 1 : 0));
|
||
xml.writeAttribute("errorCompensation", QString::number(calibParam.errorCompensation, 'f', 2));
|
||
xml.writeAttribute("correctionFactor", QString::number(calibParam.correctionFactor, 'f', 8));
|
||
|
||
// 保存轮胎存在检测的3D ROI范围
|
||
xml.writeAttribute("wheelRoi3d_xMin", QString::number(calibParam.wheelRoi3d_xMin, 'f', 2));
|
||
xml.writeAttribute("wheelRoi3d_xMax", QString::number(calibParam.wheelRoi3d_xMax, 'f', 2));
|
||
xml.writeAttribute("wheelRoi3d_yMin", QString::number(calibParam.wheelRoi3d_yMin, 'f', 2));
|
||
xml.writeAttribute("wheelRoi3d_yMax", QString::number(calibParam.wheelRoi3d_yMax, 'f', 2));
|
||
xml.writeAttribute("wheelRoi3d_zMin", QString::number(calibParam.wheelRoi3d_zMin, 'f', 2));
|
||
xml.writeAttribute("wheelRoi3d_zMax", QString::number(calibParam.wheelRoi3d_zMax, 'f', 2));
|
||
|
||
// 保存planeCalib矩阵
|
||
QStringList planeCalibList;
|
||
for (int i = 0; i < 9; ++i) {
|
||
planeCalibList.append(QString::number(calibParam.planeCalib[i], 'f', 8));
|
||
}
|
||
xml.writeAttribute("planeCalib", planeCalibList.join(","));
|
||
|
||
// 保存invRMatrix矩阵
|
||
QStringList invRMatrixList;
|
||
for (int i = 0; i < 9; ++i) {
|
||
invRMatrixList.append(QString::number(calibParam.invRMatrix[i], 'f', 8));
|
||
}
|
||
xml.writeAttribute("invRMatrix", invRMatrixList.join(","));
|
||
|
||
xml.writeEndElement(); // CameraCalib
|
||
}
|
||
xml.writeEndElement(); // PlaneCalibParams
|
||
|
||
// 保存算法参数
|
||
xml.writeStartElement("AlgorithmParams");
|
||
|
||
// 角点参数
|
||
xml.writeStartElement("CornerParam");
|
||
xml.writeAttribute("minEndingGap", QString::number(configResult.algorithmParams.cornerParam.minEndingGap));
|
||
xml.writeAttribute("minEndingGap_z", QString::number(configResult.algorithmParams.cornerParam.minEndingGap_z));
|
||
xml.writeAttribute("scale", QString::number(configResult.algorithmParams.cornerParam.scale));
|
||
xml.writeAttribute("cornerTh", QString::number(configResult.algorithmParams.cornerParam.cornerTh));
|
||
xml.writeAttribute("jumpCornerTh_1", QString::number(configResult.algorithmParams.cornerParam.jumpCornerTh_1));
|
||
xml.writeAttribute("jumpCornerTh_2", QString::number(configResult.algorithmParams.cornerParam.jumpCornerTh_2));
|
||
xml.writeEndElement(); // CornerParam
|
||
|
||
// 线段参数
|
||
xml.writeStartElement("LineSegParam");
|
||
xml.writeAttribute("segGapTh_y", QString::number(configResult.algorithmParams.lineSegParam.segGapTh_y));
|
||
xml.writeAttribute("segGapTh_z", QString::number(configResult.algorithmParams.lineSegParam.segGapTh_z));
|
||
xml.writeAttribute("maxDist", QString::number(configResult.algorithmParams.lineSegParam.maxDist));
|
||
xml.writeEndElement(); // LineSegParam
|
||
|
||
// 离群点过滤参数
|
||
xml.writeStartElement("OutlierFilterParam");
|
||
xml.writeAttribute("continuityTh", QString::number(configResult.algorithmParams.filterParam.continuityTh));
|
||
xml.writeAttribute("outlierTh", QString::number(configResult.algorithmParams.filterParam.outlierTh));
|
||
xml.writeEndElement(); // OutlierFilterParam
|
||
|
||
// 树生长参数
|
||
xml.writeStartElement("TreeGrowParam");
|
||
xml.writeAttribute("yDeviation_max", QString::number(configResult.algorithmParams.growParam.yDeviation_max));
|
||
xml.writeAttribute("zDeviation_max", QString::number(configResult.algorithmParams.growParam.zDeviation_max));
|
||
xml.writeAttribute("maxLineSkipNum", QString::number(configResult.algorithmParams.growParam.maxLineSkipNum));
|
||
xml.writeAttribute("maxSkipDistance", QString::number(configResult.algorithmParams.growParam.maxSkipDistance));
|
||
xml.writeAttribute("minLTypeTreeLen", QString::number(configResult.algorithmParams.growParam.minLTypeTreeLen));
|
||
xml.writeAttribute("minVTypeTreeLen", QString::number(configResult.algorithmParams.growParam.minVTypeTreeLen));
|
||
xml.writeEndElement(); // TreeGrowParam
|
||
|
||
xml.writeEndElement(); // AlgorithmParams
|
||
|
||
// 保存调试参数
|
||
xml.writeStartElement("DebugParam");
|
||
xml.writeAttribute("enableDebug", QString::number(configResult.debugParam.enableDebug));
|
||
xml.writeAttribute("savePointCloud", QString::number(configResult.debugParam.savePointCloud));
|
||
xml.writeAttribute("saveDebugImage", QString::number(configResult.debugParam.saveDebugImage));
|
||
xml.writeAttribute("printDetailLog", QString::number(configResult.debugParam.printDetailLog));
|
||
xml.writeAttribute("debugOutputPath", QString::fromStdString(configResult.debugParam.debugOutputPath));
|
||
xml.writeEndElement(); // DebugParam
|
||
|
||
// 保存服务端配置
|
||
xml.writeStartElement("LocalServerConfig");
|
||
xml.writeStartElement("ServerPort");
|
||
xml.writeAttribute("port", QString::number(configResult.serverPort));
|
||
xml.writeEndElement(); // ServerPort
|
||
xml.writeStartElement("TcpPort");
|
||
xml.writeAttribute("port", QString::number(configResult.tcpPort));
|
||
xml.writeEndElement(); // TcpPort
|
||
xml.writeEndElement(); // LocalServerConfig
|
||
|
||
// 保存服务器列表
|
||
xml.writeStartElement("Servers");
|
||
for (const auto& server : configResult.servers) {
|
||
xml.writeStartElement("Server");
|
||
xml.writeAttribute("name", QString::fromStdString(server.name));
|
||
xml.writeAttribute("ip", QString::fromStdString(server.ip));
|
||
xml.writeAttribute("port", QString::number(server.port));
|
||
xml.writeEndElement(); // Server
|
||
}
|
||
xml.writeEndElement(); // Servers
|
||
|
||
xml.writeEndElement(); // WheelMeasureConfig
|
||
xml.writeEndDocument();
|
||
|
||
file.close();
|
||
|
||
// 通知配置改变
|
||
if (m_notify) {
|
||
m_notify->OnConfigChanged(configResult);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
void VrWheelMeasureConfig::SetConfigChangeNotify(IVrWheelMeasureConfigChangeNotify* notify)
|
||
{
|
||
m_notify = notify;
|
||
}
|