313 lines
13 KiB
C++
313 lines
13 KiB
C++
#include "VrConfig.h"
|
||
|
||
#include <iostream>
|
||
#include <string>
|
||
#include <vector>
|
||
|
||
#include "ConfigXmlUtils.h"
|
||
#include "VrLog.h"
|
||
|
||
using namespace tinyxml2;
|
||
|
||
namespace {
|
||
|
||
// 读取一段 <ScrewParam>/<CornerParam>/<FilterParam>/<GrowParam> 子节点到 VrAlgorithmParams。
|
||
// planeCalibParam 不在这里处理——它是相机级共享配置，在外层单独加载。
|
||
void LoadAlgorithmParamsBlock(XMLElement* algoElement, VrAlgorithmParams& params)
|
||
{
|
||
if (!algoElement) return;
|
||
|
||
if (XMLElement* e = algoElement->FirstChildElement("ScrewParam")) {
|
||
if (e->Attribute("rodDiameter"))
|
||
params.screwParam.rodDiameter = e->DoubleAttribute("rodDiameter");
|
||
if (e->Attribute("isHorizonScan"))
|
||
params.screwParam.isHorizonScan = e->BoolAttribute("isHorizonScan");
|
||
}
|
||
if (XMLElement* e = algoElement->FirstChildElement("FilterParam")) {
|
||
if (e->Attribute("continuityTh"))
|
||
params.filterParam.continuityTh = e->DoubleAttribute("continuityTh");
|
||
if (e->Attribute("outlierTh"))
|
||
params.filterParam.outlierTh = e->DoubleAttribute("outlierTh");
|
||
}
|
||
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");
|
||
}
|
||
if (XMLElement* e = algoElement->FirstChildElement("GrowParam")) {
|
||
if (e->Attribute("yDeviation_max"))
|
||
params.growParam.yDeviation_max = e->DoubleAttribute("yDeviation_max");
|
||
if (e->Attribute("zDeviation_max"))
|
||
params.growParam.zDeviation_max = e->DoubleAttribute("zDeviation_max");
|
||
if (e->Attribute("maxLineSkipNum"))
|
||
params.growParam.maxLineSkipNum = e->IntAttribute("maxLineSkipNum");
|
||
if (e->Attribute("maxSkipDistance"))
|
||
params.growParam.maxSkipDistance = e->DoubleAttribute("maxSkipDistance");
|
||
if (e->Attribute("minLTypeTreeLen"))
|
||
params.growParam.minLTypeTreeLen = e->DoubleAttribute("minLTypeTreeLen");
|
||
if (e->Attribute("minVTypeTreeLen"))
|
||
params.growParam.minVTypeTreeLen = e->DoubleAttribute("minVTypeTreeLen");
|
||
}
|
||
}
|
||
|
||
void SaveAlgorithmParamsBlock(XMLDocument& doc, XMLElement* algoElement, const VrAlgorithmParams& params)
|
||
{
|
||
XMLElement* screwElement = doc.NewElement("ScrewParam");
|
||
screwElement->SetAttribute("rodDiameter", params.screwParam.rodDiameter);
|
||
screwElement->SetAttribute("isHorizonScan", params.screwParam.isHorizonScan);
|
||
algoElement->InsertEndChild(screwElement);
|
||
|
||
XMLElement* filterElement = doc.NewElement("FilterParam");
|
||
filterElement->SetAttribute("continuityTh", params.filterParam.continuityTh);
|
||
filterElement->SetAttribute("outlierTh", params.filterParam.outlierTh);
|
||
algoElement->InsertEndChild(filterElement);
|
||
|
||
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);
|
||
|
||
XMLElement* growElement = doc.NewElement("GrowParam");
|
||
growElement->SetAttribute("maxLineSkipNum", params.growParam.maxLineSkipNum);
|
||
growElement->SetAttribute("yDeviation_max", params.growParam.yDeviation_max);
|
||
growElement->SetAttribute("maxSkipDistance", params.growParam.maxSkipDistance);
|
||
growElement->SetAttribute("zDeviation_max", params.growParam.zDeviation_max);
|
||
growElement->SetAttribute("minLTypeTreeLen", params.growParam.minLTypeTreeLen);
|
||
growElement->SetAttribute("minVTypeTreeLen", params.growParam.minVTypeTreeLen);
|
||
algoElement->InsertEndChild(growElement);
|
||
}
|
||
|
||
// 读取一个 <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_TOOL_DISK) ? "toolDisk" : "screw";
|
||
}
|
||
|
||
DetectionType StrToDetectionType(const char* s)
|
||
{
|
||
if (!s) return DETECTION_TYPE_SCREW;
|
||
return (std::string(s) == "toolDisk") ? DETECTION_TYPE_TOOL_DISK : DETECTION_TYPE_SCREW;
|
||
}
|
||
|
||
} // 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()) != "ScrewPositionConfig")
|
||
{
|
||
LOG_ERR("Config file format error: root element is not ScrewPositionConfig\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");
|
||
}
|
||
}
|
||
|
||
// 缺失槽位补全默认值，保证 (cameraIndex ∈ cameraList) × (Screw|ToolDisk) 全覆盖
|
||
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_SCREW);
|
||
configResult.EnsureDetectionConfig(camIdx, DETECTION_TYPE_TOOL_DISK);
|
||
}
|
||
|
||
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("ScrewPositionConfig");
|
||
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);
|
||
|
||
// 兼容老消费者，保留 <TCP port=.../> 节点
|
||
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;
|
||
}
|