434 lines
18 KiB
C++
434 lines
18 KiB
C++
#include "VrConfig.h"
|
|
#include <iostream>
|
|
#include <vector>
|
|
#include <string>
|
|
#include <sstream>
|
|
|
|
#include "VrLog.h"
|
|
|
|
using namespace tinyxml2;
|
|
|
|
CVrConfig::CVrConfig() : m_pNotify(nullptr)
|
|
{
|
|
// 构造函数
|
|
}
|
|
|
|
CVrConfig::~CVrConfig()
|
|
{
|
|
// 析构函数
|
|
}
|
|
|
|
int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
|
|
{
|
|
// 使用tinyxml2库加载XML文件
|
|
XMLDocument doc;
|
|
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;
|
|
}
|
|
|
|
// 1. 解析相机设备列表
|
|
XMLElement* camerasElement = root->FirstChildElement("Cameras");
|
|
if (camerasElement)
|
|
{
|
|
configResult.cameraList.clear();
|
|
XMLElement* cameraElement = camerasElement->FirstChildElement("Camera");
|
|
while (cameraElement)
|
|
{
|
|
DeviceInfo camera;
|
|
if (cameraElement->Attribute("name"))
|
|
camera.name = cameraElement->Attribute("name");
|
|
if (cameraElement->Attribute("ip"))
|
|
camera.ip = cameraElement->Attribute("ip");
|
|
cameraElement->QueryIntAttribute("index", &camera.index);
|
|
|
|
configResult.cameraList.push_back(camera);
|
|
LOG_INFO("Camera: name=%s, ip=%s, index=%d\n",
|
|
camera.name.c_str(), camera.ip.c_str(), camera.index);
|
|
|
|
cameraElement = cameraElement->NextSiblingElement("Camera");
|
|
}
|
|
}
|
|
|
|
// 2. 解析调试参数
|
|
XMLElement* debugElement = root->FirstChildElement("Debug");
|
|
if (debugElement)
|
|
{
|
|
debugElement->QueryBoolAttribute("enableDebug", &configResult.debugParam.enableDebug);
|
|
debugElement->QueryBoolAttribute("saveDebugImage", &configResult.debugParam.saveDebugImage);
|
|
debugElement->QueryBoolAttribute("savePointCloud", &configResult.debugParam.savePointCloud);
|
|
debugElement->QueryBoolAttribute("printDetailLog", &configResult.debugParam.printDetailLog);
|
|
|
|
if (debugElement->Attribute("debugOutputPath"))
|
|
configResult.debugParam.debugOutputPath = debugElement->Attribute("debugOutputPath");
|
|
|
|
LOG_INFO("Debug config: enableDebug=%d, saveDebugImage=%d, savePointCloud=%d, debugOutputPath=%s\n",
|
|
configResult.debugParam.enableDebug,
|
|
configResult.debugParam.saveDebugImage,
|
|
configResult.debugParam.savePointCloud,
|
|
configResult.debugParam.debugOutputPath.c_str());
|
|
}
|
|
|
|
// 3. 解析串口配置
|
|
XMLElement* serialElement = root->FirstChildElement("Serial");
|
|
if (serialElement)
|
|
{
|
|
if (serialElement->Attribute("portName"))
|
|
configResult.serialConfig.portName = serialElement->Attribute("portName");
|
|
|
|
serialElement->QueryIntAttribute("baudRate", &configResult.serialConfig.baudRate);
|
|
serialElement->QueryIntAttribute("dataBits", &configResult.serialConfig.dataBits);
|
|
serialElement->QueryIntAttribute("stopBits", &configResult.serialConfig.stopBits);
|
|
serialElement->QueryIntAttribute("parity", &configResult.serialConfig.parity);
|
|
serialElement->QueryIntAttribute("flowControl", &configResult.serialConfig.flowControl);
|
|
serialElement->QueryBoolAttribute("enabled", &configResult.serialConfig.enabled);
|
|
|
|
LOG_INFO("Serial config: port=%s, baudRate=%d, enabled=%d\n",
|
|
configResult.serialConfig.portName.c_str(),
|
|
configResult.serialConfig.baudRate,
|
|
configResult.serialConfig.enabled);
|
|
}
|
|
|
|
// 4. 解析TCP端口
|
|
XMLElement* tcpElement = root->FirstChildElement("TCP");
|
|
if (tcpElement)
|
|
{
|
|
int port = 5020;
|
|
tcpElement->QueryIntAttribute("port", &port);
|
|
configResult.tcpPort = static_cast<uint16_t>(port);
|
|
LOG_INFO("TCP port: %d\n", configResult.tcpPort);
|
|
}
|
|
|
|
// 5. 解析螺杆检测参数
|
|
XMLElement* screwElement = root->FirstChildElement("ScrewParam");
|
|
if (screwElement)
|
|
{
|
|
XMLElement* elem = screwElement->FirstChildElement("RodDiameter");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.screwParam.rodDiameter);
|
|
|
|
elem = screwElement->FirstChildElement("IsHorizonScan");
|
|
if (elem) {
|
|
const char* text = elem->GetText();
|
|
if (text) configResult.algorithmParams.screwParam.isHorizonScan = (std::string(text) == "true");
|
|
}
|
|
|
|
LOG_INFO("ScrewParam: rodDiameter=%.1f, isHorizonScan=%d\n",
|
|
configResult.algorithmParams.screwParam.rodDiameter,
|
|
configResult.algorithmParams.screwParam.isHorizonScan);
|
|
}
|
|
|
|
// 6. 解析角点检测参数
|
|
XMLElement* cornerElement = root->FirstChildElement("CornerParam");
|
|
if (cornerElement)
|
|
{
|
|
XMLElement* elem = cornerElement->FirstChildElement("MinEndingGap");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.cornerParam.minEndingGap);
|
|
|
|
elem = cornerElement->FirstChildElement("MinEndingGapZ");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.cornerParam.minEndingGap_z);
|
|
|
|
elem = cornerElement->FirstChildElement("Scale");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.cornerParam.scale);
|
|
|
|
elem = cornerElement->FirstChildElement("CornerTh");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.cornerParam.cornerTh);
|
|
|
|
elem = cornerElement->FirstChildElement("JumpCornerTh1");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.cornerParam.jumpCornerTh_1);
|
|
|
|
elem = cornerElement->FirstChildElement("JumpCornerTh2");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.cornerParam.jumpCornerTh_2);
|
|
|
|
LOG_INFO("CornerParam: cornerTh=%.1f, scale=%.1f\n",
|
|
configResult.algorithmParams.cornerParam.cornerTh,
|
|
configResult.algorithmParams.cornerParam.scale);
|
|
}
|
|
|
|
// 7. 解析离群点滤波参数
|
|
XMLElement* filterElement = root->FirstChildElement("FilterParam");
|
|
if (filterElement)
|
|
{
|
|
XMLElement* elem = filterElement->FirstChildElement("MeanK");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.filterParam.continuityTh);
|
|
|
|
elem = filterElement->FirstChildElement("StddevMul");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.filterParam.outlierTh);
|
|
|
|
LOG_INFO("FilterParam: continuityTh=%.1f, outlierTh=%.1f\n",
|
|
configResult.algorithmParams.filterParam.continuityTh,
|
|
configResult.algorithmParams.filterParam.outlierTh);
|
|
}
|
|
|
|
// 8. 解析区域生长参数
|
|
XMLElement* growElement = root->FirstChildElement("GrowParam");
|
|
if (growElement)
|
|
{
|
|
XMLElement* elem = growElement->FirstChildElement("YDeviationMax");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.growParam.yDeviation_max);
|
|
|
|
elem = growElement->FirstChildElement("ZDeviationMax");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.growParam.zDeviation_max);
|
|
|
|
elem = growElement->FirstChildElement("MaxLineSkipNum");
|
|
if (elem) elem->QueryIntText(&configResult.algorithmParams.growParam.maxLineSkipNum);
|
|
|
|
elem = growElement->FirstChildElement("MaxSkipDistance");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.growParam.maxSkipDistance);
|
|
|
|
elem = growElement->FirstChildElement("MinLTypeTreeLen");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.growParam.minLTypeTreeLen);
|
|
|
|
elem = growElement->FirstChildElement("MinVTypeTreeLen");
|
|
if (elem) elem->QueryDoubleText(&configResult.algorithmParams.growParam.minVTypeTreeLen);
|
|
|
|
LOG_INFO("GrowParam: yDeviation_max=%.1f, zDeviation_max=%.1f\n",
|
|
configResult.algorithmParams.growParam.yDeviation_max,
|
|
configResult.algorithmParams.growParam.zDeviation_max);
|
|
}
|
|
|
|
// 9. 解析平面校准参数
|
|
XMLElement* calibElement = root->FirstChildElement("CalibrationConfig");
|
|
while (calibElement)
|
|
{
|
|
int cameraId = 1;
|
|
calibElement->QueryIntAttribute("id", &cameraId);
|
|
|
|
VrCameraPlaneCalibParam calibParam;
|
|
calibParam.cameraIndex = cameraId;
|
|
|
|
XMLElement* elem = calibElement->FirstChildElement("PlaneHeight");
|
|
if (elem) elem->QueryDoubleText(&calibParam.planeHeight);
|
|
|
|
elem = calibElement->FirstChildElement("IsCalibrated");
|
|
if (elem) {
|
|
const char* text = elem->GetText();
|
|
if (text) calibParam.isCalibrated = (std::string(text) == "true");
|
|
}
|
|
|
|
// 解析 PlaneCalib 矩阵 (9个元素)
|
|
elem = calibElement->FirstChildElement("PlaneCalib");
|
|
if (elem) {
|
|
const char* text = elem->GetText();
|
|
if (text) {
|
|
std::istringstream iss(text);
|
|
for (int i = 0; i < 9 && iss; i++) {
|
|
iss >> calibParam.planeCalib[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
// 解析 InvRMatrix 矩阵 (9个元素)
|
|
elem = calibElement->FirstChildElement("InvRMatrix");
|
|
if (elem) {
|
|
const char* text = elem->GetText();
|
|
if (text) {
|
|
std::istringstream iss(text);
|
|
for (int i = 0; i < 9 && iss; i++) {
|
|
iss >> calibParam.invRMatrix[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(calibParam);
|
|
LOG_INFO("CalibrationConfig: cameraId=%d, planeHeight=%.3f, isCalibrated=%d\n",
|
|
cameraId, calibParam.planeHeight, calibParam.isCalibrated);
|
|
|
|
calibElement = calibElement->NextSiblingElement("CalibrationConfig");
|
|
}
|
|
|
|
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;
|
|
|
|
// 创建 XML 声明
|
|
XMLDeclaration* declaration = doc.NewDeclaration();
|
|
doc.InsertFirstChild(declaration);
|
|
|
|
// 创建根元素
|
|
XMLElement* root = doc.NewElement("ScrewPositionConfig");
|
|
doc.InsertEndChild(root);
|
|
|
|
// 1. 保存相机设备列表
|
|
if (!configResult.cameraList.empty())
|
|
{
|
|
XMLElement* camerasElement = doc.NewElement("Cameras");
|
|
for (const auto& camera : configResult.cameraList)
|
|
{
|
|
XMLElement* cameraElement = doc.NewElement("Camera");
|
|
cameraElement->SetAttribute("name", camera.name.c_str());
|
|
cameraElement->SetAttribute("ip", camera.ip.c_str());
|
|
cameraElement->SetAttribute("index", camera.index);
|
|
camerasElement->InsertEndChild(cameraElement);
|
|
}
|
|
root->InsertEndChild(camerasElement);
|
|
}
|
|
|
|
// 2. 保存调试参数
|
|
XMLElement* debugElement = doc.NewElement("Debug");
|
|
debugElement->SetAttribute("enableDebug", configResult.debugParam.enableDebug);
|
|
debugElement->SetAttribute("saveDebugImage", configResult.debugParam.saveDebugImage);
|
|
debugElement->SetAttribute("savePointCloud", configResult.debugParam.savePointCloud);
|
|
debugElement->SetAttribute("printDetailLog", configResult.debugParam.printDetailLog);
|
|
debugElement->SetAttribute("debugOutputPath", configResult.debugParam.debugOutputPath.c_str());
|
|
root->InsertEndChild(debugElement);
|
|
|
|
// 3. 保存串口配置
|
|
XMLElement* serialElement = doc.NewElement("Serial");
|
|
serialElement->SetAttribute("portName", configResult.serialConfig.portName.c_str());
|
|
serialElement->SetAttribute("baudRate", configResult.serialConfig.baudRate);
|
|
serialElement->SetAttribute("dataBits", configResult.serialConfig.dataBits);
|
|
serialElement->SetAttribute("stopBits", configResult.serialConfig.stopBits);
|
|
serialElement->SetAttribute("parity", configResult.serialConfig.parity);
|
|
serialElement->SetAttribute("flowControl", configResult.serialConfig.flowControl);
|
|
serialElement->SetAttribute("enabled", configResult.serialConfig.enabled);
|
|
root->InsertEndChild(serialElement);
|
|
|
|
// 4. 保存TCP端口
|
|
XMLElement* tcpElement = doc.NewElement("TCP");
|
|
tcpElement->SetAttribute("port", configResult.tcpPort);
|
|
root->InsertEndChild(tcpElement);
|
|
|
|
// 5. 保存螺杆检测参数
|
|
XMLElement* screwElement = doc.NewElement("ScrewParam");
|
|
XMLElement* elem = doc.NewElement("RodDiameter");
|
|
elem->SetText(configResult.algorithmParams.screwParam.rodDiameter);
|
|
screwElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("IsHorizonScan");
|
|
elem->SetText(configResult.algorithmParams.screwParam.isHorizonScan ? "true" : "false");
|
|
screwElement->InsertEndChild(elem);
|
|
root->InsertEndChild(screwElement);
|
|
|
|
// 6. 保存角点检测参数
|
|
XMLElement* cornerElement = doc.NewElement("CornerParam");
|
|
elem = doc.NewElement("MinEndingGap");
|
|
elem->SetText(configResult.algorithmParams.cornerParam.minEndingGap);
|
|
cornerElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("MinEndingGapZ");
|
|
elem->SetText(configResult.algorithmParams.cornerParam.minEndingGap_z);
|
|
cornerElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("Scale");
|
|
elem->SetText(configResult.algorithmParams.cornerParam.scale);
|
|
cornerElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("CornerTh");
|
|
elem->SetText(configResult.algorithmParams.cornerParam.cornerTh);
|
|
cornerElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("JumpCornerTh1");
|
|
elem->SetText(configResult.algorithmParams.cornerParam.jumpCornerTh_1);
|
|
cornerElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("JumpCornerTh2");
|
|
elem->SetText(configResult.algorithmParams.cornerParam.jumpCornerTh_2);
|
|
cornerElement->InsertEndChild(elem);
|
|
root->InsertEndChild(cornerElement);
|
|
|
|
// 7. 保存离群点滤波参数
|
|
XMLElement* filterElement = doc.NewElement("FilterParam");
|
|
elem = doc.NewElement("MeanK");
|
|
elem->SetText(configResult.algorithmParams.filterParam.continuityTh);
|
|
filterElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("StddevMul");
|
|
elem->SetText(configResult.algorithmParams.filterParam.outlierTh);
|
|
filterElement->InsertEndChild(elem);
|
|
root->InsertEndChild(filterElement);
|
|
|
|
// 8. 保存区域生长参数
|
|
XMLElement* growElement = doc.NewElement("GrowParam");
|
|
elem = doc.NewElement("YDeviationMax");
|
|
elem->SetText(configResult.algorithmParams.growParam.yDeviation_max);
|
|
growElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("ZDeviationMax");
|
|
elem->SetText(configResult.algorithmParams.growParam.zDeviation_max);
|
|
growElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("MaxLineSkipNum");
|
|
elem->SetText(configResult.algorithmParams.growParam.maxLineSkipNum);
|
|
growElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("MaxSkipDistance");
|
|
elem->SetText(configResult.algorithmParams.growParam.maxSkipDistance);
|
|
growElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("MinLTypeTreeLen");
|
|
elem->SetText(configResult.algorithmParams.growParam.minLTypeTreeLen);
|
|
growElement->InsertEndChild(elem);
|
|
elem = doc.NewElement("MinVTypeTreeLen");
|
|
elem->SetText(configResult.algorithmParams.growParam.minVTypeTreeLen);
|
|
growElement->InsertEndChild(elem);
|
|
root->InsertEndChild(growElement);
|
|
|
|
// 9. 保存平面校准参数
|
|
for (const auto& calibParam : configResult.algorithmParams.planeCalibParam.cameraCalibParams)
|
|
{
|
|
XMLElement* calibElement = doc.NewElement("CalibrationConfig");
|
|
calibElement->SetAttribute("id", calibParam.cameraIndex);
|
|
|
|
elem = doc.NewElement("PlaneHeight");
|
|
elem->SetText(calibParam.planeHeight);
|
|
calibElement->InsertEndChild(elem);
|
|
|
|
elem = doc.NewElement("IsCalibrated");
|
|
elem->SetText(calibParam.isCalibrated ? "true" : "false");
|
|
calibElement->InsertEndChild(elem);
|
|
|
|
// 保存 PlaneCalib 矩阵
|
|
std::ostringstream ossPlane;
|
|
for (int i = 0; i < 9; i++) {
|
|
if (i > 0) ossPlane << " ";
|
|
ossPlane << calibParam.planeCalib[i];
|
|
}
|
|
elem = doc.NewElement("PlaneCalib");
|
|
elem->SetText(ossPlane.str().c_str());
|
|
calibElement->InsertEndChild(elem);
|
|
|
|
// 保存 InvRMatrix 矩阵
|
|
std::ostringstream ossInv;
|
|
for (int i = 0; i < 9; i++) {
|
|
if (i > 0) ossInv << " ";
|
|
ossInv << calibParam.invRMatrix[i];
|
|
}
|
|
elem = doc.NewElement("InvRMatrix");
|
|
elem->SetText(ossInv.str().c_str());
|
|
calibElement->InsertEndChild(elem);
|
|
|
|
root->InsertEndChild(calibElement);
|
|
}
|
|
|
|
// 保存到文件
|
|
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;
|
|
}
|
|
|
|
LOG_INFO("Config saved successfully to: %s\n", filePath.c_str());
|
|
return true;
|
|
}
|
|
|
|
// 设置配置改变通知回调
|
|
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
|
|
{
|
|
m_pNotify = notify;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief 创建实例
|
|
* @return 实例
|
|
*/
|
|
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
|
|
{
|
|
*ppVrConfig = new CVrConfig() ;
|
|
return *ppVrConfig != nullptr;
|
|
}
|