266 lines
10 KiB
C++
266 lines
10 KiB
C++
#include "VrConfig.h"
|
|
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "ConfigXmlUtils.h"
|
|
#include "VrLog.h"
|
|
|
|
using namespace tinyxml2;
|
|
|
|
namespace {
|
|
|
|
// 读取 CornerParam 子节点到 VrAlgorithmParams
|
|
void LoadAlgorithmParamsBlock(XMLElement* algoElement, VrAlgorithmParams& params)
|
|
{
|
|
if (!algoElement) return;
|
|
|
|
if (XMLElement* e = algoElement->FirstChildElement("CornerParam")) {
|
|
if (e->Attribute("minEndingGap"))
|
|
params.cornerParam.minEndingGap = e->DoubleAttribute("minEndingGap");
|
|
if (e->Attribute("minEndingGap_z"))
|
|
params.cornerParam.minEndingGap_z = e->DoubleAttribute("minEndingGap_z");
|
|
if (e->Attribute("scale"))
|
|
params.cornerParam.scale = e->DoubleAttribute("scale");
|
|
if (e->Attribute("cornerTh"))
|
|
params.cornerParam.cornerTh = e->DoubleAttribute("cornerTh");
|
|
if (e->Attribute("jumpCornerTh_1"))
|
|
params.cornerParam.jumpCornerTh_1 = e->DoubleAttribute("jumpCornerTh_1");
|
|
if (e->Attribute("jumpCornerTh_2"))
|
|
params.cornerParam.jumpCornerTh_2 = e->DoubleAttribute("jumpCornerTh_2");
|
|
}
|
|
}
|
|
|
|
void SaveAlgorithmParamsBlock(XMLDocument& doc, XMLElement* algoElement, const VrAlgorithmParams& params)
|
|
{
|
|
XMLElement* cornerElement = doc.NewElement("CornerParam");
|
|
cornerElement->SetAttribute("cornerTh", params.cornerParam.cornerTh);
|
|
cornerElement->SetAttribute("scale", params.cornerParam.scale);
|
|
cornerElement->SetAttribute("minEndingGap", params.cornerParam.minEndingGap);
|
|
cornerElement->SetAttribute("minEndingGap_z", params.cornerParam.minEndingGap_z);
|
|
cornerElement->SetAttribute("jumpCornerTh_1", params.cornerParam.jumpCornerTh_1);
|
|
cornerElement->SetAttribute("jumpCornerTh_2", params.cornerParam.jumpCornerTh_2);
|
|
algoElement->InsertEndChild(cornerElement);
|
|
}
|
|
|
|
// 读取一个 <HandEyeCalibMatrix .../> 节点的所有属性到目标对象
|
|
void LoadHandEyeMatrixAttributes(XMLElement* e, VrHandEyeCalibMatrix& m)
|
|
{
|
|
if (!e) return;
|
|
if (e->Attribute("cameraIndex")) m.cameraIndex = e->IntAttribute("cameraIndex");
|
|
for (int i = 0; i < 16; ++i) {
|
|
char attrName[16];
|
|
std::snprintf(attrName, sizeof(attrName), "m%d", i);
|
|
if (e->Attribute(attrName)) m.matrix[i] = e->DoubleAttribute(attrName);
|
|
}
|
|
if (e->Attribute("eulerOrder")) m.eulerOrder = e->IntAttribute("eulerOrder");
|
|
if (e->Attribute("rotX")) m.rotX = e->DoubleAttribute("rotX");
|
|
if (e->Attribute("rotY")) m.rotY = e->DoubleAttribute("rotY");
|
|
if (e->Attribute("rotZ")) m.rotZ = e->DoubleAttribute("rotZ");
|
|
if (e->Attribute("approachOffset")) m.approachOffset = e->DoubleAttribute("approachOffset");
|
|
if (e->Attribute("offsetX")) m.offsetX = e->DoubleAttribute("offsetX");
|
|
if (e->Attribute("offsetY")) m.offsetY = e->DoubleAttribute("offsetY");
|
|
if (e->Attribute("offsetZ")) m.offsetZ = e->DoubleAttribute("offsetZ");
|
|
if (e->Attribute("outRotX")) m.outRotX = e->DoubleAttribute("outRotX");
|
|
if (e->Attribute("outRotY")) m.outRotY = e->DoubleAttribute("outRotY");
|
|
if (e->Attribute("outRotZ")) m.outRotZ = e->DoubleAttribute("outRotZ");
|
|
}
|
|
|
|
void SaveHandEyeMatrixAttributes(XMLElement* e, const VrHandEyeCalibMatrix& m)
|
|
{
|
|
e->SetAttribute("cameraIndex", m.cameraIndex);
|
|
for (int i = 0; i < 16; ++i) {
|
|
char attrName[16];
|
|
std::snprintf(attrName, sizeof(attrName), "m%d", i);
|
|
e->SetAttribute(attrName, m.matrix[i]);
|
|
}
|
|
e->SetAttribute("eulerOrder", m.eulerOrder);
|
|
e->SetAttribute("rotX", m.rotX);
|
|
e->SetAttribute("rotY", m.rotY);
|
|
e->SetAttribute("rotZ", m.rotZ);
|
|
e->SetAttribute("approachOffset", m.approachOffset);
|
|
e->SetAttribute("offsetX", m.offsetX);
|
|
e->SetAttribute("offsetY", m.offsetY);
|
|
e->SetAttribute("offsetZ", m.offsetZ);
|
|
e->SetAttribute("outRotX", m.outRotX);
|
|
e->SetAttribute("outRotY", m.outRotY);
|
|
e->SetAttribute("outRotZ", m.outRotZ);
|
|
}
|
|
|
|
const char* DetectionTypeToStr(DetectionType t)
|
|
{
|
|
return (t == DETECTION_TYPE_DISC_RACK) ? "discRack" : "discHole";
|
|
}
|
|
|
|
DetectionType StrToDetectionType(const char* s)
|
|
{
|
|
if (!s) return DETECTION_TYPE_DISC_HOLE;
|
|
return (std::string(s) == "discRack") ? DETECTION_TYPE_DISC_RACK : DETECTION_TYPE_DISC_HOLE;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
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()) != "DiscHolePoseConfig")
|
|
{
|
|
LOG_ERR("Config file format error: root element is not DiscHolePoseConfig\n");
|
|
return LOAD_CONFIG_INVALID_FORMAT;
|
|
}
|
|
|
|
ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);
|
|
|
|
// 共享平面校准：相机级配置
|
|
ConfigXmlUtils::LoadPlaneCalibParams(root, configResult.algorithmParams.planeCalibParam);
|
|
|
|
// 读取四套 (camera, type) 配置
|
|
configResult.detectionConfigList.clear();
|
|
XMLElement* setsRoot = root->FirstChildElement("DetectionConfigs");
|
|
if (setsRoot) {
|
|
XMLElement* setElem = setsRoot->FirstChildElement("DetectionConfig");
|
|
while (setElem) {
|
|
VrDetectionConfigSet item;
|
|
if (setElem->Attribute("cameraIndex"))
|
|
item.cameraIndex = setElem->IntAttribute("cameraIndex");
|
|
if (setElem->Attribute("detectionType"))
|
|
item.detectionType = StrToDetectionType(setElem->Attribute("detectionType"));
|
|
|
|
// handEye 矩阵子节点
|
|
if (XMLElement* heElem = setElem->FirstChildElement("HandEyeCalibMatrix")) {
|
|
LoadHandEyeMatrixAttributes(heElem, item.handEyeMatrix);
|
|
}
|
|
// 算法参数子块
|
|
LoadAlgorithmParamsBlock(setElem->FirstChildElement("AlgorithmParams"), item.algorithmParams);
|
|
|
|
// 同步 handEyeMatrix.cameraIndex
|
|
item.handEyeMatrix.cameraIndex = item.cameraIndex;
|
|
|
|
configResult.detectionConfigList.push_back(item);
|
|
setElem = setElem->NextSiblingElement("DetectionConfig");
|
|
}
|
|
}
|
|
|
|
// 缺失槽位补全默认值
|
|
int cameraCount = static_cast<int>(configResult.cameraList.size());
|
|
if (cameraCount <= 0) cameraCount = 1;
|
|
for (int camIdx = 1; camIdx <= cameraCount; ++camIdx) {
|
|
configResult.EnsureDetectionConfig(camIdx, DETECTION_TYPE_DISC_HOLE);
|
|
configResult.EnsureDetectionConfig(camIdx, DETECTION_TYPE_DISC_RACK);
|
|
}
|
|
|
|
ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);
|
|
ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);
|
|
|
|
if (XMLElement* networkElement = root->FirstChildElement("NetworkConfig")) {
|
|
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 (XMLElement* tcpElement = root->FirstChildElement("TCP")) {
|
|
if (tcpElement->Attribute("port"))
|
|
configResult.tcpPort = static_cast<uint16_t>(tcpElement->IntAttribute("port"));
|
|
}
|
|
|
|
LOG_INFO("Config loaded successfully from: %s (detection sets: %zu)\n",
|
|
filePath.c_str(), configResult.detectionConfigList.size());
|
|
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("DiscHolePoseConfig");
|
|
doc.InsertEndChild(root);
|
|
|
|
ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);
|
|
|
|
// 共享平面校准
|
|
ConfigXmlUtils::SavePlaneCalibParams(doc, root, configResult.algorithmParams.planeCalibParam);
|
|
|
|
// 四套 (camera, type) 配置
|
|
XMLElement* setsRoot = doc.NewElement("DetectionConfigs");
|
|
for (const auto& item : configResult.detectionConfigList) {
|
|
XMLElement* setElem = doc.NewElement("DetectionConfig");
|
|
setElem->SetAttribute("cameraIndex", item.cameraIndex);
|
|
setElem->SetAttribute("detectionType", DetectionTypeToStr(item.detectionType));
|
|
|
|
XMLElement* heElem = doc.NewElement("HandEyeCalibMatrix");
|
|
VrHandEyeCalibMatrix syncedMatrix = item.handEyeMatrix;
|
|
syncedMatrix.cameraIndex = item.cameraIndex;
|
|
SaveHandEyeMatrixAttributes(heElem, syncedMatrix);
|
|
setElem->InsertEndChild(heElem);
|
|
|
|
XMLElement* algoElem = doc.NewElement("AlgorithmParams");
|
|
SaveAlgorithmParamsBlock(doc, algoElem, item.algorithmParams);
|
|
setElem->InsertEndChild(algoElem);
|
|
|
|
setsRoot->InsertEndChild(setElem);
|
|
}
|
|
root->InsertEndChild(setsRoot);
|
|
|
|
ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);
|
|
ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);
|
|
|
|
XMLElement* networkElement = doc.NewElement("NetworkConfig");
|
|
networkElement->SetAttribute("tcpServerPort", configResult.tcpPort);
|
|
networkElement->SetAttribute("poseOutputOrder", configResult.poseOutputOrder);
|
|
networkElement->SetAttribute("byteOrder", configResult.byteOrder);
|
|
root->InsertEndChild(networkElement);
|
|
|
|
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;
|
|
}
|