403 lines
21 KiB
C++

#include "VrConfig.h"
#include <iostream>
#include <vector>
#include <string>
#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()) != "BagThreadPositionConfig")
{
LOG_ERR("Config file format error: root element is not BagThreadPositionConfig\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* algoElement = root->FirstChildElement("AlgorithmParams");
if (algoElement)
{
// 2.1 解析线头参数 (ThreadParam)
XMLElement* threadElement = algoElement->FirstChildElement("ThreadParam");
if (threadElement)
{
threadElement->QueryBoolAttribute("isHorizonScan", &configResult.algorithmParams.threadParam.isHorizonScan);
threadElement->QueryBoolAttribute("scanFromThreadHead", &configResult.algorithmParams.threadParam.scanFromThreadHead);
threadElement->QueryDoubleAttribute("stitchWidth", &configResult.algorithmParams.threadParam.stitchWidth);
threadElement->QueryDoubleAttribute("operateDist", &configResult.algorithmParams.threadParam.operateDist);
LOG_INFO("ThreadParam: isHorizonScan=%d, stitchWidth=%.1f, operateDist=%.1f\n",
configResult.algorithmParams.threadParam.isHorizonScan,
configResult.algorithmParams.threadParam.stitchWidth,
configResult.algorithmParams.threadParam.operateDist);
}
// 2.2 解析离群点滤波参数 (FilterParam)
XMLElement* filterElement = algoElement->FirstChildElement("FilterParam");
if (filterElement)
{
filterElement->QueryDoubleAttribute("continuityTh", &configResult.algorithmParams.filterParam.continuityTh);
filterElement->QueryDoubleAttribute("outlierTh", &configResult.algorithmParams.filterParam.outlierTh);
LOG_INFO("FilterParam: continuityTh=%.1f, outlierTh=%.1f\n",
configResult.algorithmParams.filterParam.continuityTh,
configResult.algorithmParams.filterParam.outlierTh);
}
// 2.3 解析角点检测参数 (CornerParam)
XMLElement* cornerElement = algoElement->FirstChildElement("CornerParam");
if (cornerElement)
{
cornerElement->QueryDoubleAttribute("minEndingGap", &configResult.algorithmParams.cornerParam.minEndingGap);
cornerElement->QueryDoubleAttribute("minEndingGap_z", &configResult.algorithmParams.cornerParam.minEndingGap_z);
cornerElement->QueryDoubleAttribute("scale", &configResult.algorithmParams.cornerParam.scale);
cornerElement->QueryDoubleAttribute("cornerTh", &configResult.algorithmParams.cornerParam.cornerTh);
cornerElement->QueryDoubleAttribute("jumpCornerTh_1", &configResult.algorithmParams.cornerParam.jumpCornerTh_1);
cornerElement->QueryDoubleAttribute("jumpCornerTh_2", &configResult.algorithmParams.cornerParam.jumpCornerTh_2);
LOG_INFO("CornerParam: minEndingGap=%.1f, cornerTh=%.1f\n",
configResult.algorithmParams.cornerParam.minEndingGap,
configResult.algorithmParams.cornerParam.cornerTh);
}
// 2.4 解析凸起特征参数 (RaisedFeatureParam)
XMLElement* raisedElement = algoElement->FirstChildElement("RaisedFeatureParam");
if (raisedElement)
{
raisedElement->QueryDoubleAttribute("minJumpZ", &configResult.algorithmParams.raisedFeatureParam.minJumpZ);
raisedElement->QueryDoubleAttribute("minK", &configResult.algorithmParams.raisedFeatureParam.minK);
raisedElement->QueryDoubleAttribute("widthMin", &configResult.algorithmParams.raisedFeatureParam.widthMin);
raisedElement->QueryDoubleAttribute("widthMax", &configResult.algorithmParams.raisedFeatureParam.widthMax);
LOG_INFO("RaisedFeatureParam: minJumpZ=%.1f, minK=%.2f\n",
configResult.algorithmParams.raisedFeatureParam.minJumpZ,
configResult.algorithmParams.raisedFeatureParam.minK);
}
// 2.5 解析树生长参数 (GrowParam)
XMLElement* growElement = algoElement->FirstChildElement("GrowParam");
if (growElement)
{
growElement->QueryDoubleAttribute("yDeviation_max", &configResult.algorithmParams.growParam.yDeviation_max);
growElement->QueryDoubleAttribute("zDeviation_max", &configResult.algorithmParams.growParam.zDeviation_max);
growElement->QueryIntAttribute("maxLineSkipNum", &configResult.algorithmParams.growParam.maxLineSkipNum);
growElement->QueryDoubleAttribute("maxSkipDistance", &configResult.algorithmParams.growParam.maxSkipDistance);
growElement->QueryDoubleAttribute("minLTypeTreeLen", &configResult.algorithmParams.growParam.minLTypeTreeLen);
growElement->QueryDoubleAttribute("minVTypeTreeLen", &configResult.algorithmParams.growParam.minVTypeTreeLen);
LOG_INFO("GrowParam: yDeviation_max=%.1f, zDeviation_max=%.1f\n",
configResult.algorithmParams.growParam.yDeviation_max,
configResult.algorithmParams.growParam.zDeviation_max);
}
// 2.6 解析平面标定参数 (PlaneCalibParams)
XMLElement* calibElement = algoElement->FirstChildElement("PlaneCalibParams");
if (calibElement)
{
XMLElement* cameraCalibElement = calibElement->FirstChildElement("CameraCalibParam");
while (cameraCalibElement)
{
VrCameraPlaneCalibParam cameraParam;
cameraCalibElement->QueryIntAttribute("cameraIndex", &cameraParam.cameraIndex);
if (cameraCalibElement->Attribute("cameraName"))
cameraParam.cameraName = cameraCalibElement->Attribute("cameraName");
cameraCalibElement->QueryBoolAttribute("isCalibrated", &cameraParam.isCalibrated);
cameraCalibElement->QueryDoubleAttribute("planeHeight", &cameraParam.planeHeight);
// 解析校准矩阵(展开的属性格式)
cameraCalibElement->QueryDoubleAttribute("planeCalib_00", &cameraParam.planeCalib[0]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_01", &cameraParam.planeCalib[1]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_02", &cameraParam.planeCalib[2]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_10", &cameraParam.planeCalib[3]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_11", &cameraParam.planeCalib[4]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_12", &cameraParam.planeCalib[5]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_20", &cameraParam.planeCalib[6]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_21", &cameraParam.planeCalib[7]);
cameraCalibElement->QueryDoubleAttribute("planeCalib_22", &cameraParam.planeCalib[8]);
// 解析逆矩阵(展开的属性格式)
cameraCalibElement->QueryDoubleAttribute("invRMatrix_00", &cameraParam.invRMatrix[0]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_01", &cameraParam.invRMatrix[1]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_02", &cameraParam.invRMatrix[2]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_10", &cameraParam.invRMatrix[3]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_11", &cameraParam.invRMatrix[4]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_12", &cameraParam.invRMatrix[5]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_20", &cameraParam.invRMatrix[6]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_21", &cameraParam.invRMatrix[7]);
cameraCalibElement->QueryDoubleAttribute("invRMatrix_22", &cameraParam.invRMatrix[8]);
configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam);
LOG_INFO("PlaneCalibParam: camera %d, isCalibrated=%d, planeHeight=%.3f\n",
cameraParam.cameraIndex, cameraParam.isCalibrated, cameraParam.planeHeight);
cameraCalibElement = cameraCalibElement->NextSiblingElement("CameraCalibParam");
}
}
}
// 3. 解析调试参数
XMLElement* debugElement = root->FirstChildElement("DebugParam");
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());
}
// 4. 解析串口配置
XMLElement* serialElement = root->FirstChildElement("SerialConfig");
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);
}
// 5. 解析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);
}
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("BagThreadPositionConfig");
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* algoElement = doc.NewElement("AlgorithmParams");
// 2.1 保存线头参数
XMLElement* threadElement = doc.NewElement("ThreadParam");
threadElement->SetAttribute("isHorizonScan", configResult.algorithmParams.threadParam.isHorizonScan);
threadElement->SetAttribute("scanFromThreadHead", configResult.algorithmParams.threadParam.scanFromThreadHead);
threadElement->SetAttribute("stitchWidth", configResult.algorithmParams.threadParam.stitchWidth);
threadElement->SetAttribute("operateDist", configResult.algorithmParams.threadParam.operateDist);
algoElement->InsertEndChild(threadElement);
// 2.2 保存离群点滤波参数
XMLElement* filterElement = doc.NewElement("FilterParam");
filterElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
filterElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
algoElement->InsertEndChild(filterElement);
// 2.3 保存角点检测参数
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);
// 2.4 保存凸起特征参数
XMLElement* raisedElement = doc.NewElement("RaisedFeatureParam");
raisedElement->SetAttribute("minJumpZ", configResult.algorithmParams.raisedFeatureParam.minJumpZ);
raisedElement->SetAttribute("minK", configResult.algorithmParams.raisedFeatureParam.minK);
raisedElement->SetAttribute("widthMin", configResult.algorithmParams.raisedFeatureParam.widthMin);
raisedElement->SetAttribute("widthMax", configResult.algorithmParams.raisedFeatureParam.widthMax);
algoElement->InsertEndChild(raisedElement);
// 2.5 保存树生长参数
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);
// 2.6 保存平面标定参数
if (!configResult.algorithmParams.planeCalibParam.cameraCalibParams.empty())
{
XMLElement* calibElement = doc.NewElement("PlaneCalibParams");
for (const auto& cameraParam : configResult.algorithmParams.planeCalibParam.cameraCalibParams)
{
XMLElement* cameraCalibElement = doc.NewElement("CameraCalibParam");
cameraCalibElement->SetAttribute("cameraIndex", cameraParam.cameraIndex);
cameraCalibElement->SetAttribute("cameraName", cameraParam.cameraName.c_str());
cameraCalibElement->SetAttribute("isCalibrated", cameraParam.isCalibrated);
cameraCalibElement->SetAttribute("planeHeight", cameraParam.planeHeight);
// 保存校准矩阵(展开的属性格式)
cameraCalibElement->SetAttribute("planeCalib_00", cameraParam.planeCalib[0]);
cameraCalibElement->SetAttribute("planeCalib_01", cameraParam.planeCalib[1]);
cameraCalibElement->SetAttribute("planeCalib_02", cameraParam.planeCalib[2]);
cameraCalibElement->SetAttribute("planeCalib_10", cameraParam.planeCalib[3]);
cameraCalibElement->SetAttribute("planeCalib_11", cameraParam.planeCalib[4]);
cameraCalibElement->SetAttribute("planeCalib_12", cameraParam.planeCalib[5]);
cameraCalibElement->SetAttribute("planeCalib_20", cameraParam.planeCalib[6]);
cameraCalibElement->SetAttribute("planeCalib_21", cameraParam.planeCalib[7]);
cameraCalibElement->SetAttribute("planeCalib_22", cameraParam.planeCalib[8]);
// 保存逆矩阵(展开的属性格式)
cameraCalibElement->SetAttribute("invRMatrix_00", cameraParam.invRMatrix[0]);
cameraCalibElement->SetAttribute("invRMatrix_01", cameraParam.invRMatrix[1]);
cameraCalibElement->SetAttribute("invRMatrix_02", cameraParam.invRMatrix[2]);
cameraCalibElement->SetAttribute("invRMatrix_10", cameraParam.invRMatrix[3]);
cameraCalibElement->SetAttribute("invRMatrix_11", cameraParam.invRMatrix[4]);
cameraCalibElement->SetAttribute("invRMatrix_12", cameraParam.invRMatrix[5]);
cameraCalibElement->SetAttribute("invRMatrix_20", cameraParam.invRMatrix[6]);
cameraCalibElement->SetAttribute("invRMatrix_21", cameraParam.invRMatrix[7]);
cameraCalibElement->SetAttribute("invRMatrix_22", cameraParam.invRMatrix[8]);
calibElement->InsertEndChild(cameraCalibElement);
}
algoElement->InsertEndChild(calibElement);
}
root->InsertEndChild(algoElement);
// 3. 保存调试参数
XMLElement* debugElement = doc.NewElement("DebugParam");
debugElement->SetAttribute("enableDebug", configResult.debugParam.enableDebug);
debugElement->SetAttribute("savePointCloud", configResult.debugParam.savePointCloud);
debugElement->SetAttribute("saveDebugImage", configResult.debugParam.saveDebugImage);
debugElement->SetAttribute("printDetailLog", configResult.debugParam.printDetailLog);
debugElement->SetAttribute("debugOutputPath", configResult.debugParam.debugOutputPath.c_str());
root->InsertEndChild(debugElement);
// 4. 保存串口配置
XMLElement* serialElement = doc.NewElement("SerialConfig");
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);
// 5. 保存TCP端口
XMLElement* tcpElement = doc.NewElement("TCP");
tcpElement->SetAttribute("port", configResult.tcpPort);
root->InsertEndChild(tcpElement);
// 保存到文件
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;
}