256 lines
11 KiB
C++
256 lines
11 KiB
C++
#include "VrConfig.h"
|
||
|
||
#include <iostream>
|
||
#include <string>
|
||
#include <vector>
|
||
|
||
#include "ConfigXmlUtils.h"
|
||
#include "VrLog.h"
|
||
|
||
using namespace tinyxml2;
|
||
|
||
CVrConfig::CVrConfig() : m_pNotify(nullptr)
|
||
{
|
||
}
|
||
|
||
CVrConfig::~CVrConfig()
|
||
{
|
||
}
|
||
|
||
int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
|
||
{
|
||
XMLDocument doc;
|
||
const XMLError err = doc.LoadFile(filePath.c_str());
|
||
if (err != XML_SUCCESS)
|
||
{
|
||
LOG_ERR("Failed to open config file: %s\n", filePath.c_str());
|
||
return LOAD_CONFIG_FILE_NOT_FOUND;
|
||
}
|
||
|
||
XMLElement* root = doc.RootElement();
|
||
if (!root || std::string(root->Name()) != "ScrewPositionConfig")
|
||
{
|
||
LOG_ERR("Config file format error: root element is not ScrewPositionConfig\n");
|
||
return LOAD_CONFIG_INVALID_FORMAT;
|
||
}
|
||
|
||
ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);
|
||
|
||
XMLElement* algoElement = root->FirstChildElement("AlgorithmParams");
|
||
if (algoElement)
|
||
{
|
||
XMLElement* screwElement = algoElement->FirstChildElement("ScrewParam");
|
||
if (screwElement)
|
||
{
|
||
if (screwElement->Attribute("rodDiameter"))
|
||
configResult.algorithmParams.screwParam.rodDiameter = screwElement->DoubleAttribute("rodDiameter");
|
||
if (screwElement->Attribute("isHorizonScan"))
|
||
configResult.algorithmParams.screwParam.isHorizonScan = screwElement->BoolAttribute("isHorizonScan");
|
||
}
|
||
|
||
XMLElement* filterElement = algoElement->FirstChildElement("FilterParam");
|
||
if (filterElement)
|
||
{
|
||
if (filterElement->Attribute("continuityTh"))
|
||
configResult.algorithmParams.filterParam.continuityTh = filterElement->DoubleAttribute("continuityTh");
|
||
if (filterElement->Attribute("outlierTh"))
|
||
configResult.algorithmParams.filterParam.outlierTh = filterElement->DoubleAttribute("outlierTh");
|
||
}
|
||
|
||
XMLElement* cornerElement = algoElement->FirstChildElement("CornerParam");
|
||
if (cornerElement)
|
||
{
|
||
if (cornerElement->Attribute("minEndingGap"))
|
||
configResult.algorithmParams.cornerParam.minEndingGap = cornerElement->DoubleAttribute("minEndingGap");
|
||
if (cornerElement->Attribute("minEndingGap_z"))
|
||
configResult.algorithmParams.cornerParam.minEndingGap_z = cornerElement->DoubleAttribute("minEndingGap_z");
|
||
if (cornerElement->Attribute("scale"))
|
||
configResult.algorithmParams.cornerParam.scale = cornerElement->DoubleAttribute("scale");
|
||
if (cornerElement->Attribute("cornerTh"))
|
||
configResult.algorithmParams.cornerParam.cornerTh = cornerElement->DoubleAttribute("cornerTh");
|
||
if (cornerElement->Attribute("jumpCornerTh_1"))
|
||
configResult.algorithmParams.cornerParam.jumpCornerTh_1 = cornerElement->DoubleAttribute("jumpCornerTh_1");
|
||
if (cornerElement->Attribute("jumpCornerTh_2"))
|
||
configResult.algorithmParams.cornerParam.jumpCornerTh_2 = cornerElement->DoubleAttribute("jumpCornerTh_2");
|
||
}
|
||
|
||
XMLElement* growElement = algoElement->FirstChildElement("GrowParam");
|
||
if (growElement)
|
||
{
|
||
if (growElement->Attribute("yDeviation_max"))
|
||
configResult.algorithmParams.growParam.yDeviation_max = growElement->DoubleAttribute("yDeviation_max");
|
||
if (growElement->Attribute("zDeviation_max"))
|
||
configResult.algorithmParams.growParam.zDeviation_max = growElement->DoubleAttribute("zDeviation_max");
|
||
if (growElement->Attribute("maxLineSkipNum"))
|
||
configResult.algorithmParams.growParam.maxLineSkipNum = growElement->IntAttribute("maxLineSkipNum");
|
||
if (growElement->Attribute("maxSkipDistance"))
|
||
configResult.algorithmParams.growParam.maxSkipDistance = growElement->DoubleAttribute("maxSkipDistance");
|
||
if (growElement->Attribute("minLTypeTreeLen"))
|
||
configResult.algorithmParams.growParam.minLTypeTreeLen = growElement->DoubleAttribute("minLTypeTreeLen");
|
||
if (growElement->Attribute("minVTypeTreeLen"))
|
||
configResult.algorithmParams.growParam.minVTypeTreeLen = growElement->DoubleAttribute("minVTypeTreeLen");
|
||
}
|
||
|
||
ConfigXmlUtils::LoadPlaneCalibParams(algoElement, configResult.algorithmParams.planeCalibParam);
|
||
}
|
||
|
||
ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);
|
||
ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);
|
||
|
||
// 兼容旧配置：若存在已废弃的 eulerOrder/dirVectorInvert/longAxisDir，
|
||
// 则一次性迁移为每台相机手眼矩阵的 eulerOrder/rotX/rotY/rotZ。
|
||
int legacyEulerOrder = 11;
|
||
bool hasLegacyEulerOrder = false;
|
||
double legacyRotX = 0.0;
|
||
double legacyRotY = 0.0;
|
||
double legacyRotZ = 0.0;
|
||
bool hasLegacyRotation = false;
|
||
|
||
XMLElement* networkElement = root->FirstChildElement("NetworkConfig");
|
||
if (networkElement)
|
||
{
|
||
if (networkElement->Attribute("tcpServerPort"))
|
||
configResult.tcpPort = static_cast<uint16_t>(networkElement->IntAttribute("tcpServerPort"));
|
||
if (networkElement->Attribute("poseOutputOrder"))
|
||
configResult.poseOutputOrder = networkElement->IntAttribute("poseOutputOrder");
|
||
if (networkElement->Attribute("byteOrder"))
|
||
configResult.byteOrder = networkElement->IntAttribute("byteOrder");
|
||
|
||
if (networkElement->Attribute("eulerOrder")) {
|
||
legacyEulerOrder = networkElement->IntAttribute("eulerOrder");
|
||
hasLegacyEulerOrder = true;
|
||
}
|
||
// 旧字段：方向向量调整（XY/XZ/YZ 反向），映射为绕对应轴 180°
|
||
if (networkElement->Attribute("dirVectorInvert")) {
|
||
const int dirInvert = networkElement->IntAttribute("dirVectorInvert");
|
||
switch (dirInvert) {
|
||
case 1: legacyRotZ = 180.0; hasLegacyRotation = true; break; // XY 反向 ≡ Rz(180°)
|
||
case 2: legacyRotY = 180.0; hasLegacyRotation = true; break; // XZ 反向 ≡ Ry(180°)
|
||
case 3: legacyRotX = 180.0; hasLegacyRotation = true; break; // YZ 反向 ≡ Rx(180°)
|
||
default: break;
|
||
}
|
||
}
|
||
// 旧字段：长轴对应轴（Y 轴时相当于绕 Z 旋转 -90°）
|
||
if (networkElement->Attribute("longAxisDir")) {
|
||
const int longAxisDir = networkElement->IntAttribute("longAxisDir");
|
||
if (longAxisDir == 1) {
|
||
legacyRotZ += -90.0;
|
||
hasLegacyRotation = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
XMLElement* tcpElement = root->FirstChildElement("TCP");
|
||
if (tcpElement && tcpElement->Attribute("port"))
|
||
{
|
||
configResult.tcpPort = static_cast<uint16_t>(tcpElement->IntAttribute("port"));
|
||
}
|
||
|
||
ConfigXmlUtils::LoadHandEyeCalibMatrixs(root,
|
||
configResult.handEyeCalibMatrixList,
|
||
legacyEulerOrder);
|
||
|
||
if (hasLegacyEulerOrder || hasLegacyRotation) {
|
||
for (auto& calibMatrix : configResult.handEyeCalibMatrixList) {
|
||
if (hasLegacyEulerOrder) {
|
||
calibMatrix.eulerOrder = legacyEulerOrder;
|
||
}
|
||
if (hasLegacyRotation) {
|
||
calibMatrix.rotX = legacyRotX;
|
||
calibMatrix.rotY = legacyRotY;
|
||
calibMatrix.rotZ = legacyRotZ;
|
||
}
|
||
}
|
||
}
|
||
|
||
LOG_INFO("Config loaded successfully from: %s\n", filePath.c_str());
|
||
return LOAD_CONFIG_SUCCESS;
|
||
}
|
||
|
||
bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
|
||
{
|
||
XMLDocument doc;
|
||
|
||
XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
|
||
doc.InsertFirstChild(declaration);
|
||
|
||
XMLElement* root = doc.NewElement("ScrewPositionConfig");
|
||
doc.InsertEndChild(root);
|
||
|
||
ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);
|
||
|
||
XMLElement* algoElement = doc.NewElement("AlgorithmParams");
|
||
root->InsertEndChild(algoElement);
|
||
|
||
XMLElement* screwElement = doc.NewElement("ScrewParam");
|
||
screwElement->SetAttribute("rodDiameter", configResult.algorithmParams.screwParam.rodDiameter);
|
||
screwElement->SetAttribute("isHorizonScan", configResult.algorithmParams.screwParam.isHorizonScan);
|
||
algoElement->InsertEndChild(screwElement);
|
||
|
||
XMLElement* filterElement = doc.NewElement("FilterParam");
|
||
filterElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
|
||
filterElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
|
||
algoElement->InsertEndChild(filterElement);
|
||
|
||
XMLElement* cornerElement = doc.NewElement("CornerParam");
|
||
cornerElement->SetAttribute("cornerTh", configResult.algorithmParams.cornerParam.cornerTh);
|
||
cornerElement->SetAttribute("scale", configResult.algorithmParams.cornerParam.scale);
|
||
cornerElement->SetAttribute("minEndingGap", configResult.algorithmParams.cornerParam.minEndingGap);
|
||
cornerElement->SetAttribute("minEndingGap_z", configResult.algorithmParams.cornerParam.minEndingGap_z);
|
||
cornerElement->SetAttribute("jumpCornerTh_1", configResult.algorithmParams.cornerParam.jumpCornerTh_1);
|
||
cornerElement->SetAttribute("jumpCornerTh_2", configResult.algorithmParams.cornerParam.jumpCornerTh_2);
|
||
algoElement->InsertEndChild(cornerElement);
|
||
|
||
XMLElement* growElement = doc.NewElement("GrowParam");
|
||
growElement->SetAttribute("maxLineSkipNum", configResult.algorithmParams.growParam.maxLineSkipNum);
|
||
growElement->SetAttribute("yDeviation_max", configResult.algorithmParams.growParam.yDeviation_max);
|
||
growElement->SetAttribute("maxSkipDistance", configResult.algorithmParams.growParam.maxSkipDistance);
|
||
growElement->SetAttribute("zDeviation_max", configResult.algorithmParams.growParam.zDeviation_max);
|
||
growElement->SetAttribute("minLTypeTreeLen", configResult.algorithmParams.growParam.minLTypeTreeLen);
|
||
growElement->SetAttribute("minVTypeTreeLen", configResult.algorithmParams.growParam.minVTypeTreeLen);
|
||
algoElement->InsertEndChild(growElement);
|
||
|
||
ConfigXmlUtils::SavePlaneCalibParams(doc, algoElement, configResult.algorithmParams.planeCalibParam);
|
||
ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);
|
||
ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);
|
||
|
||
ConfigXmlUtils::SaveHandEyeCalibMatrixs(doc, root, configResult.handEyeCalibMatrixList);
|
||
|
||
XMLElement* networkElement = doc.NewElement("NetworkConfig");
|
||
networkElement->SetAttribute("tcpServerPort", configResult.tcpPort);
|
||
networkElement->SetAttribute("poseOutputOrder", configResult.poseOutputOrder);
|
||
networkElement->SetAttribute("byteOrder", configResult.byteOrder);
|
||
root->InsertEndChild(networkElement);
|
||
|
||
// Keep the legacy TCP node for backward compatibility.
|
||
XMLElement* tcpElement = doc.NewElement("TCP");
|
||
tcpElement->SetAttribute("port", configResult.tcpPort);
|
||
root->InsertEndChild(tcpElement);
|
||
|
||
const XMLError err = doc.SaveFile(filePath.c_str());
|
||
if (err != XML_SUCCESS)
|
||
{
|
||
LOG_ERR("Failed to save config file: %s\n", filePath.c_str());
|
||
return false;
|
||
}
|
||
|
||
if (m_pNotify)
|
||
{
|
||
m_pNotify->OnConfigChanged(configResult);
|
||
}
|
||
|
||
LOG_INFO("Config saved successfully to: %s\n", filePath.c_str());
|
||
return true;
|
||
}
|
||
|
||
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
|
||
{
|
||
m_pNotify = notify;
|
||
}
|
||
|
||
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
|
||
{
|
||
*ppVrConfig = new CVrConfig();
|
||
return *ppVrConfig != nullptr;
|
||
}
|