snsopush/GrabBag
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
GrabBag/VrConfig/Src/VrConfig.cpp

677 lines
36 KiB
C++
Raw Permalink Normal View History

支持带方向的编织袋检测
2025-07-23 01:35:14 +08:00
#include "VrConfig.h"
#include <iostream>
#include <vector>
#include <string>
using namespace tinyxml2;
CVrConfig::CVrConfig() : m_pNotify(nullptr)
{
// 构造函数
}
CVrConfig::~CVrConfig()
{
// 析构函数
}
ConfigResult CVrConfig::LoadConfig(const std::string& filePath)
{
ConfigResult result;
// 使用tinyxml2库加载XML文件
XMLDocument doc;
XMLError err = doc.LoadFile(filePath.c_str());
if (err != XML_SUCCESS)
{
std::cerr << "open config file failed: " << filePath << std::endl;
return result;
}
// 获取根元素
XMLElement* root = doc.RootElement();
if (!root || std::string(root->Name()) != "GrabBagConfig")
{
std::cerr << "config file format error: root element is not GrabBagConfig" << std::endl;
return result;
}
// 解析摄像头列表
XMLElement* camerasElement = root->FirstChildElement("Cameras");
if (camerasElement)
{
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");
result.cameraList.push_back(camera);
cameraElement = cameraElement->NextSiblingElement("Camera");
}
}
// 解析设备列表
XMLElement* devicesElement = root->FirstChildElement("Devices");
if (devicesElement)
{
XMLElement* deviceElement = devicesElement->FirstChildElement("Device");
while (deviceElement)
{
DeviceInfo device;
if (deviceElement->Attribute("name"))
device.name = deviceElement->Attribute("name");
if (deviceElement->Attribute("ip"))
device.ip = deviceElement->Attribute("ip");
result.deviceList.push_back(device);
deviceElement = deviceElement->NextSiblingElement("Device");
}
}
// 解析算法参数
XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
if (algoParamsElement)
{
// 解析编织袋参数
XMLElement* bagParamElement = algoParamsElement->FirstChildElement("BagParam");
if (bagParamElement)
{
if (bagParamElement->Attribute("bagL"))
result.algorithmParams.bagParam.bagL = bagParamElement->DoubleAttribute("bagL");
if (bagParamElement->Attribute("bagW"))
result.algorithmParams.bagParam.bagW = bagParamElement->DoubleAttribute("bagW");
if (bagParamElement->Attribute("bagH"))
result.algorithmParams.bagParam.bagH = bagParamElement->DoubleAttribute("bagH");
}
// 解析垛参数
XMLElement* pileParamElement = algoParamsElement->FirstChildElement("PileParam");
if (pileParamElement)
{
if (pileParamElement->Attribute("pileL"))
result.algorithmParams.pileParam.pileL = pileParamElement->DoubleAttribute("pileL");
if (pileParamElement->Attribute("pileW"))
result.algorithmParams.pileParam.pileW = pileParamElement->DoubleAttribute("pileW");
if (pileParamElement->Attribute("pileH"))
result.algorithmParams.pileParam.pileH = pileParamElement->DoubleAttribute("pileH");
}
// 解析滤波参数
XMLElement* filterParamElement = algoParamsElement->FirstChildElement("FilterParam");
if (filterParamElement)
{
if (filterParamElement->Attribute("continuityTh"))
result.algorithmParams.filterParam.continuityTh = filterParamElement->DoubleAttribute("continuityTh");
if (filterParamElement->Attribute("outlierTh"))
result.algorithmParams.filterParam.outlierTh = filterParamElement->IntAttribute("outlierTh");
}
// 解析角点特征参数
XMLElement* cornerParamElement = algoParamsElement->FirstChildElement("CornerParam");
if (cornerParamElement)
{
if (cornerParamElement->Attribute("minEndingGap"))
result.algorithmParams.cornerParam.minEndingGap = cornerParamElement->DoubleAttribute("minEndingGap");
if (cornerParamElement->Attribute("minEndingGap_z"))
result.algorithmParams.cornerParam.minEndingGap_z = cornerParamElement->DoubleAttribute("minEndingGap_z");
if (cornerParamElement->Attribute("scale"))
result.algorithmParams.cornerParam.scale = cornerParamElement->DoubleAttribute("scale");
if (cornerParamElement->Attribute("cornerTh"))
result.algorithmParams.cornerParam.cornerTh = cornerParamElement->DoubleAttribute("cornerTh");
if (cornerParamElement->Attribute("jumpCornerTh_1"))
result.algorithmParams.cornerParam.jumpCornerTh_1 = cornerParamElement->DoubleAttribute("jumpCornerTh_1");
if (cornerParamElement->Attribute("jumpCornerTh_2"))
result.algorithmParams.cornerParam.jumpCornerTh_2 = cornerParamElement->DoubleAttribute("jumpCornerTh_2");
}
// 解析斜率参数
XMLElement* slopeParamElement = algoParamsElement->FirstChildElement("SlopeParam");
if (slopeParamElement)
{
if (slopeParamElement->Attribute("LSlopeZWin"))
result.algorithmParams.slopeParam.LSlopeZWin = slopeParamElement->DoubleAttribute("LSlopeZWin");
if (slopeParamElement->Attribute("validSlopeH"))
result.algorithmParams.slopeParam.validSlopeH = slopeParamElement->DoubleAttribute("validSlopeH");
if (slopeParamElement->Attribute("minLJumpH"))
result.algorithmParams.slopeParam.minLJumpH = slopeParamElement->DoubleAttribute("minLJumpH");
if (slopeParamElement->Attribute("minEndingGap"))
result.algorithmParams.slopeParam.minEndingGap = slopeParamElement->DoubleAttribute("minEndingGap");
}
// 解析山谷参数
XMLElement* valleyParamElement = algoParamsElement->FirstChildElement("ValleyParam");
if (valleyParamElement)
{
if (valleyParamElement->Attribute("valleyMinH"))
result.algorithmParams.valleyParam.valleyMinH = valleyParamElement->DoubleAttribute("valleyMinH");
if (valleyParamElement->Attribute("valleyMaxW"))
result.algorithmParams.valleyParam.valleyMaxW = valleyParamElement->DoubleAttribute("valleyMaxW");
}
// 解析增长参数
XMLElement* growParamElement = algoParamsElement->FirstChildElement("GrowParam");
if (growParamElement)
{
if (growParamElement->Attribute("maxLineSkipNum"))
result.algorithmParams.growParam.maxLineSkipNum = growParamElement->IntAttribute("maxLineSkipNum");
if (growParamElement->Attribute("yDeviation_max"))
result.algorithmParams.growParam.yDeviation_max = growParamElement->DoubleAttribute("yDeviation_max");
if (growParamElement->Attribute("maxSkipDistance"))
result.algorithmParams.growParam.maxSkipDistance = growParamElement->DoubleAttribute("maxSkipDistance");
if (growParamElement->Attribute("zDeviation_max"))
result.algorithmParams.growParam.zDeviation_max = growParamElement->DoubleAttribute("zDeviation_max");
if (growParamElement->Attribute("minLTypeTreeLen"))
result.algorithmParams.growParam.minLTypeTreeLen = growParamElement->DoubleAttribute("minLTypeTreeLen");
if (growParamElement->Attribute("minVTypeTreeLen"))
result.algorithmParams.growParam.minVTypeTreeLen = growParamElement->DoubleAttribute("minVTypeTreeLen");
}
// 解析HSV颜色比较参数
XMLElement* hsvCmpParamElement = algoParamsElement->FirstChildElement("HsvCmpParam");
if (hsvCmpParamElement)
{
if (hsvCmpParamElement->Attribute("hueTh"))
result.algorithmParams.hsvCmpParam.hueTh = hsvCmpParamElement->DoubleAttribute("hueTh");
if (hsvCmpParamElement->Attribute("saturateTh"))
result.algorithmParams.hsvCmpParam.saturateTh = hsvCmpParamElement->DoubleAttribute("saturateTh");
if (hsvCmpParamElement->Attribute("FBVldPtRatioTh"))
result.algorithmParams.hsvCmpParam.FBVldPtRatioTh = hsvCmpParamElement->DoubleAttribute("FBVldPtRatioTh");
if (hsvCmpParamElement->Attribute("frontVldPtGreater"))
result.algorithmParams.hsvCmpParam.frontVldPtGreater = hsvCmpParamElement->BoolAttribute("frontVldPtGreater");
if (hsvCmpParamElement->Attribute("front_upVldPtGreater"))
result.algorithmParams.hsvCmpParam.front_upVldPtGreater = hsvCmpParamElement->BoolAttribute("front_upVldPtGreater");
if (hsvCmpParamElement->Attribute("back_upVldPtGreater"))
result.algorithmParams.hsvCmpParam.back_upVldPtGreater = hsvCmpParamElement->BoolAttribute("back_upVldPtGreater");
}
// 解析RGB颜色模式
XMLElement* rgbColorPatternElement = algoParamsElement->FirstChildElement("RgbColorPattern");
if (rgbColorPatternElement)
{
if (rgbColorPatternElement->Attribute("r"))
result.algorithmParams.rgbColorPattern.r = rgbColorPatternElement->IntAttribute("r");
if (rgbColorPatternElement->Attribute("g"))
result.algorithmParams.rgbColorPattern.g = rgbColorPatternElement->IntAttribute("g");
if (rgbColorPatternElement->Attribute("b"))
result.algorithmParams.rgbColorPattern.b = rgbColorPatternElement->IntAttribute("b");
}
// 解析颜色模板参数
XMLElement* colorTemplateParamElement = algoParamsElement->FirstChildElement("ColorTemplateParam");
if (colorTemplateParamElement)
{
// 解析正面颜色模板
XMLElement* frontColorTemplateElement = colorTemplateParamElement->FirstChildElement("FrontColorTemplate");
if (frontColorTemplateElement && frontColorTemplateElement->GetText())
{
std::string frontColorStr = frontColorTemplateElement->GetText();
ParseColorTemplate(frontColorStr, result.algorithmParams.colorTemplateParam.frontColorTemplate);
}
// 解析反面颜色模板
XMLElement* backColorTemplateElement = colorTemplateParamElement->FirstChildElement("BackColorTemplate");
if (backColorTemplateElement && backColorTemplateElement->GetText())
{
std::string backColorStr = backColorTemplateElement->GetText();
ParseColorTemplate(backColorStr, result.algorithmParams.colorTemplateParam.backColorTemplate);
}
}
// 解析多相机平面校准参数
XMLElement* planeCalibParamsElement = algoParamsElement->FirstChildElement("PlaneCalibParams");
if (planeCalibParamsElement)
{
XMLElement* cameraCalibParamElement = planeCalibParamsElement->FirstChildElement("CameraCalibParam");
while (cameraCalibParamElement)
{
VrCameraPlaneCalibParam cameraParam;
// 读取相机基础信息
if (cameraCalibParamElement->Attribute("cameraIndex"))
cameraParam.cameraIndex = cameraCalibParamElement->IntAttribute("cameraIndex");
if (cameraCalibParamElement->Attribute("cameraName"))
cameraParam.cameraName = cameraCalibParamElement->Attribute("cameraName");
if (cameraCalibParamElement->Attribute("isCalibrated"))
cameraParam.isCalibrated = cameraCalibParamElement->BoolAttribute("isCalibrated");
if (cameraCalibParamElement->Attribute("planeHeight"))
cameraParam.planeHeight = cameraCalibParamElement->DoubleAttribute("planeHeight");
// 读取旋转矩阵planeCalib[9] (3x3矩阵)
if (cameraCalibParamElement->Attribute("planeCalib_00"))
cameraParam.planeCalib[0] = cameraCalibParamElement->DoubleAttribute("planeCalib_00");
if (cameraCalibParamElement->Attribute("planeCalib_01"))
cameraParam.planeCalib[1] = cameraCalibParamElement->DoubleAttribute("planeCalib_01");
if (cameraCalibParamElement->Attribute("planeCalib_02"))
cameraParam.planeCalib[2] = cameraCalibParamElement->DoubleAttribute("planeCalib_02");
if (cameraCalibParamElement->Attribute("planeCalib_10"))
cameraParam.planeCalib[3] = cameraCalibParamElement->DoubleAttribute("planeCalib_10");
if (cameraCalibParamElement->Attribute("planeCalib_11"))
cameraParam.planeCalib[4] = cameraCalibParamElement->DoubleAttribute("planeCalib_11");
if (cameraCalibParamElement->Attribute("planeCalib_12"))
cameraParam.planeCalib[5] = cameraCalibParamElement->DoubleAttribute("planeCalib_12");
if (cameraCalibParamElement->Attribute("planeCalib_20"))
cameraParam.planeCalib[6] = cameraCalibParamElement->DoubleAttribute("planeCalib_20");
if (cameraCalibParamElement->Attribute("planeCalib_21"))
cameraParam.planeCalib[7] = cameraCalibParamElement->DoubleAttribute("planeCalib_21");
if (cameraCalibParamElement->Attribute("planeCalib_22"))
cameraParam.planeCalib[8] = cameraCalibParamElement->DoubleAttribute("planeCalib_22");
// 读取逆旋转矩阵invRMatrix[9] (3x3矩阵)
if (cameraCalibParamElement->Attribute("invRMatrix_00"))
cameraParam.invRMatrix[0] = cameraCalibParamElement->DoubleAttribute("invRMatrix_00");
if (cameraCalibParamElement->Attribute("invRMatrix_01"))
cameraParam.invRMatrix[1] = cameraCalibParamElement->DoubleAttribute("invRMatrix_01");
if (cameraCalibParamElement->Attribute("invRMatrix_02"))
cameraParam.invRMatrix[2] = cameraCalibParamElement->DoubleAttribute("invRMatrix_02");
if (cameraCalibParamElement->Attribute("invRMatrix_10"))
cameraParam.invRMatrix[3] = cameraCalibParamElement->DoubleAttribute("invRMatrix_10");
if (cameraCalibParamElement->Attribute("invRMatrix_11"))
cameraParam.invRMatrix[4] = cameraCalibParamElement->DoubleAttribute("invRMatrix_11");
if (cameraCalibParamElement->Attribute("invRMatrix_12"))
cameraParam.invRMatrix[5] = cameraCalibParamElement->DoubleAttribute("invRMatrix_12");
if (cameraCalibParamElement->Attribute("invRMatrix_20"))
cameraParam.invRMatrix[6] = cameraCalibParamElement->DoubleAttribute("invRMatrix_20");
if (cameraCalibParamElement->Attribute("invRMatrix_21"))
cameraParam.invRMatrix[7] = cameraCalibParamElement->DoubleAttribute("invRMatrix_21");
if (cameraCalibParamElement->Attribute("invRMatrix_22"))
cameraParam.invRMatrix[8] = cameraCalibParamElement->DoubleAttribute("invRMatrix_22");
// 添加到结果中
result.algorithmParams.planeCalibParam.cameraCalibParams.push_back(cameraParam);
// 移动到下一个相机配置
cameraCalibParamElement = cameraCalibParamElement->NextSiblingElement("CameraCalibParam");
}
}
// 兼容性处理：如果存在旧格式的PlaneCalibParam，转换为新格式
XMLElement* oldPlaneCalibParamElement = algoParamsElement->FirstChildElement("PlaneCalibParam");
if (oldPlaneCalibParamElement && result.algorithmParams.planeCalibParam.cameraCalibParams.empty())
{
VrCameraPlaneCalibParam cameraParam;
cameraParam.cameraIndex = 1; // 默认为第一个相机
cameraParam.cameraName = "默认相机";
cameraParam.isCalibrated = false;
if (oldPlaneCalibParamElement->Attribute("planeHeight"))
cameraParam.planeHeight = oldPlaneCalibParamElement->DoubleAttribute("planeHeight");
// 读取旋转矩阵
if (oldPlaneCalibParamElement->Attribute("planeCalib_00"))
cameraParam.planeCalib[0] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_00");
if (oldPlaneCalibParamElement->Attribute("planeCalib_01"))
cameraParam.planeCalib[1] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_01");
if (oldPlaneCalibParamElement->Attribute("planeCalib_02"))
cameraParam.planeCalib[2] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_02");
if (oldPlaneCalibParamElement->Attribute("planeCalib_10"))
cameraParam.planeCalib[3] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_10");
if (oldPlaneCalibParamElement->Attribute("planeCalib_11"))
cameraParam.planeCalib[4] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_11");
if (oldPlaneCalibParamElement->Attribute("planeCalib_12"))
cameraParam.planeCalib[5] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_12");
if (oldPlaneCalibParamElement->Attribute("planeCalib_20"))
cameraParam.planeCalib[6] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_20");
if (oldPlaneCalibParamElement->Attribute("planeCalib_21"))
cameraParam.planeCalib[7] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_21");
if (oldPlaneCalibParamElement->Attribute("planeCalib_22"))
cameraParam.planeCalib[8] = oldPlaneCalibParamElement->DoubleAttribute("planeCalib_22");
// 读取逆旋转矩阵
if (oldPlaneCalibParamElement->Attribute("invRMatrix_00"))
cameraParam.invRMatrix[0] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_00");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_01"))
cameraParam.invRMatrix[1] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_01");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_02"))
cameraParam.invRMatrix[2] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_02");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_10"))
cameraParam.invRMatrix[3] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_10");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_11"))
cameraParam.invRMatrix[4] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_11");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_12"))
cameraParam.invRMatrix[5] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_12");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_20"))
cameraParam.invRMatrix[6] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_20");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_21"))
cameraParam.invRMatrix[7] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_21");
if (oldPlaneCalibParamElement->Attribute("invRMatrix_22"))
cameraParam.invRMatrix[8] = oldPlaneCalibParamElement->DoubleAttribute("invRMatrix_22");
result.algorithmParams.planeCalibParam.cameraCalibParams.push_back(cameraParam);
}
}
// 解析调试参数（在AlgorithmParams外面）
XMLElement* debugParamElement = root->FirstChildElement("DebugParam");
if (debugParamElement)
{
if (debugParamElement->Attribute("enableDebug"))
result.debugParam.enableDebug = debugParamElement->BoolAttribute("enableDebug");
if (debugParamElement->Attribute("savePointCloud"))
result.debugParam.savePointCloud = debugParamElement->BoolAttribute("savePointCloud");
if (debugParamElement->Attribute("saveDebugImage"))
result.debugParam.saveDebugImage = debugParamElement->BoolAttribute("saveDebugImage");
if (debugParamElement->Attribute("printDetailLog"))
result.debugParam.printDetailLog = debugParamElement->BoolAttribute("printDetailLog");
if (debugParamElement->Attribute("debugOutputPath"))
result.debugParam.debugOutputPath = debugParamElement->Attribute("debugOutputPath");
}
// 解析项目类型（默认为拆包）
result.projectType = ProjectType::GrabBag; // 设置默认值
XMLElement* projectTypeElement = root->FirstChildElement("ProjectType");
if (projectTypeElement)
{
if (projectTypeElement->Attribute("type"))
{
std::string typeStr = projectTypeElement->Attribute("type");
result.projectType = StringToProjectType(typeStr);
}
}
// 解析串口配置
XMLElement* serialConfigElement = root->FirstChildElement("SerialConfig");
if (serialConfigElement)
{
if (serialConfigElement->Attribute("portName"))
result.serialConfig.portName = serialConfigElement->Attribute("portName");
if (serialConfigElement->Attribute("baudRate"))
result.serialConfig.baudRate = serialConfigElement->IntAttribute("baudRate");
if (serialConfigElement->Attribute("dataBits"))
result.serialConfig.dataBits = serialConfigElement->IntAttribute("dataBits");
if (serialConfigElement->Attribute("stopBits"))
result.serialConfig.stopBits = serialConfigElement->IntAttribute("stopBits");
if (serialConfigElement->Attribute("parity"))
result.serialConfig.parity = serialConfigElement->IntAttribute("parity");
if (serialConfigElement->Attribute("flowControl"))
result.serialConfig.flowControl = serialConfigElement->IntAttribute("flowControl");
if (serialConfigElement->Attribute("enabled"))
result.serialConfig.enabled = serialConfigElement->BoolAttribute("enabled");
}
return result;
}
bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
{
// 创建XML文档
XMLDocument doc;
// 添加声明
XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
doc.InsertFirstChild(declaration);
// 创建根元素
XMLElement* root = doc.NewElement("GrabBagConfig");
doc.InsertEndChild(root);
// 添加摄像头列表
XMLElement* camerasElement = doc.NewElement("Cameras");
root->InsertEndChild(camerasElement);
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());
camerasElement->InsertEndChild(cameraElement);
}
// 添加设备列表
XMLElement* devicesElement = doc.NewElement("Devices");
root->InsertEndChild(devicesElement);
for (const auto& device : configResult.deviceList)
{
XMLElement* deviceElement = doc.NewElement("Device");
deviceElement->SetAttribute("name", device.name.c_str());
deviceElement->SetAttribute("ip", device.ip.c_str());
devicesElement->InsertEndChild(deviceElement);
}
// 添加算法参数
XMLElement* algoParamsElement = doc.NewElement("AlgorithmParams");
root->InsertEndChild(algoParamsElement);
// 添加编织袋参数
XMLElement* bagParamElement = doc.NewElement("BagParam");
bagParamElement->SetAttribute("bagL", configResult.algorithmParams.bagParam.bagL);
bagParamElement->SetAttribute("bagW", configResult.algorithmParams.bagParam.bagW);
bagParamElement->SetAttribute("bagH", configResult.algorithmParams.bagParam.bagH);
algoParamsElement->InsertEndChild(bagParamElement);
// 添加垛参数
XMLElement* pileParamElement = doc.NewElement("PileParam");
pileParamElement->SetAttribute("pileL", configResult.algorithmParams.pileParam.pileL);
pileParamElement->SetAttribute("pileW", configResult.algorithmParams.pileParam.pileW);
pileParamElement->SetAttribute("pileH", configResult.algorithmParams.pileParam.pileH);
algoParamsElement->InsertEndChild(pileParamElement);
// 添加滤波参数
XMLElement* filterParamElement = doc.NewElement("FilterParam");
filterParamElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
filterParamElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
algoParamsElement->InsertEndChild(filterParamElement);
// 添加角点特征参数
XMLElement* cornerParamElement = doc.NewElement("CornerParam");
cornerParamElement->SetAttribute("minEndingGap", configResult.algorithmParams.cornerParam.minEndingGap);
cornerParamElement->SetAttribute("minEndingGap_z", configResult.algorithmParams.cornerParam.minEndingGap_z);
cornerParamElement->SetAttribute("scale", configResult.algorithmParams.cornerParam.scale);
cornerParamElement->SetAttribute("cornerTh", configResult.algorithmParams.cornerParam.cornerTh);
cornerParamElement->SetAttribute("jumpCornerTh_1", configResult.algorithmParams.cornerParam.jumpCornerTh_1);
cornerParamElement->SetAttribute("jumpCornerTh_2", configResult.algorithmParams.cornerParam.jumpCornerTh_2);
algoParamsElement->InsertEndChild(cornerParamElement);
// 添加斜率参数
XMLElement* slopeParamElement = doc.NewElement("SlopeParam");
slopeParamElement->SetAttribute("LSlopeZWin", configResult.algorithmParams.slopeParam.LSlopeZWin);
slopeParamElement->SetAttribute("validSlopeH", configResult.algorithmParams.slopeParam.validSlopeH);
slopeParamElement->SetAttribute("minLJumpH", configResult.algorithmParams.slopeParam.minLJumpH);
slopeParamElement->SetAttribute("minEndingGap", configResult.algorithmParams.slopeParam.minEndingGap);
algoParamsElement->InsertEndChild(slopeParamElement);
// 添加山谷参数
XMLElement* valleyParamElement = doc.NewElement("ValleyParam");
valleyParamElement->SetAttribute("valleyMinH", configResult.algorithmParams.valleyParam.valleyMinH);
valleyParamElement->SetAttribute("valleyMaxW", configResult.algorithmParams.valleyParam.valleyMaxW);
algoParamsElement->InsertEndChild(valleyParamElement);
// 添加增长参数
XMLElement* growParamElement = doc.NewElement("GrowParam");
growParamElement->SetAttribute("maxLineSkipNum", configResult.algorithmParams.growParam.maxLineSkipNum);
growParamElement->SetAttribute("yDeviation_max", configResult.algorithmParams.growParam.yDeviation_max);
growParamElement->SetAttribute("maxSkipDistance", configResult.algorithmParams.growParam.maxSkipDistance);
growParamElement->SetAttribute("zDeviation_max", configResult.algorithmParams.growParam.zDeviation_max);
growParamElement->SetAttribute("minLTypeTreeLen", configResult.algorithmParams.growParam.minLTypeTreeLen);
growParamElement->SetAttribute("minVTypeTreeLen", configResult.algorithmParams.growParam.minVTypeTreeLen);
algoParamsElement->InsertEndChild(growParamElement);
// 添加多相机平面校准参数
XMLElement* planeCalibParamsElement = doc.NewElement("PlaneCalibParams");
for (const auto& cameraParam : configResult.algorithmParams.planeCalibParam.cameraCalibParams)
{
XMLElement* cameraCalibParamElement = doc.NewElement("CameraCalibParam");
cameraCalibParamElement->SetAttribute("cameraIndex", cameraParam.cameraIndex);
cameraCalibParamElement->SetAttribute("cameraName", cameraParam.cameraName.c_str());
cameraCalibParamElement->SetAttribute("isCalibrated", cameraParam.isCalibrated);
cameraCalibParamElement->SetAttribute("planeHeight", cameraParam.planeHeight);
// 保存旋转矩阵planeCalib[9] (3x3矩阵)
cameraCalibParamElement->SetAttribute("planeCalib_00", cameraParam.planeCalib[0]);
cameraCalibParamElement->SetAttribute("planeCalib_01", cameraParam.planeCalib[1]);
cameraCalibParamElement->SetAttribute("planeCalib_02", cameraParam.planeCalib[2]);
cameraCalibParamElement->SetAttribute("planeCalib_10", cameraParam.planeCalib[3]);
cameraCalibParamElement->SetAttribute("planeCalib_11", cameraParam.planeCalib[4]);
cameraCalibParamElement->SetAttribute("planeCalib_12", cameraParam.planeCalib[5]);
cameraCalibParamElement->SetAttribute("planeCalib_20", cameraParam.planeCalib[6]);
cameraCalibParamElement->SetAttribute("planeCalib_21", cameraParam.planeCalib[7]);
cameraCalibParamElement->SetAttribute("planeCalib_22", cameraParam.planeCalib[8]);
// 保存逆旋转矩阵invRMatrix[9] (3x3矩阵)
cameraCalibParamElement->SetAttribute("invRMatrix_00", cameraParam.invRMatrix[0]);
cameraCalibParamElement->SetAttribute("invRMatrix_01", cameraParam.invRMatrix[1]);
cameraCalibParamElement->SetAttribute("invRMatrix_02", cameraParam.invRMatrix[2]);
cameraCalibParamElement->SetAttribute("invRMatrix_10", cameraParam.invRMatrix[3]);
cameraCalibParamElement->SetAttribute("invRMatrix_11", cameraParam.invRMatrix[4]);
cameraCalibParamElement->SetAttribute("invRMatrix_12", cameraParam.invRMatrix[5]);
cameraCalibParamElement->SetAttribute("invRMatrix_20", cameraParam.invRMatrix[6]);
cameraCalibParamElement->SetAttribute("invRMatrix_21", cameraParam.invRMatrix[7]);
cameraCalibParamElement->SetAttribute("invRMatrix_22", cameraParam.invRMatrix[8]);
planeCalibParamsElement->InsertEndChild(cameraCalibParamElement);
}
algoParamsElement->InsertEndChild(planeCalibParamsElement);
// 添加HSV颜色比较参数
XMLElement* hsvCmpParamElement = doc.NewElement("HsvCmpParam");
hsvCmpParamElement->SetAttribute("hueTh", configResult.algorithmParams.hsvCmpParam.hueTh);
hsvCmpParamElement->SetAttribute("saturateTh", configResult.algorithmParams.hsvCmpParam.saturateTh);
hsvCmpParamElement->SetAttribute("FBVldPtRatioTh", configResult.algorithmParams.hsvCmpParam.FBVldPtRatioTh);
hsvCmpParamElement->SetAttribute("frontVldPtGreater", configResult.algorithmParams.hsvCmpParam.frontVldPtGreater);
hsvCmpParamElement->SetAttribute("front_upVldPtGreater", configResult.algorithmParams.hsvCmpParam.front_upVldPtGreater);
hsvCmpParamElement->SetAttribute("back_upVldPtGreater", configResult.algorithmParams.hsvCmpParam.back_upVldPtGreater);
algoParamsElement->InsertEndChild(hsvCmpParamElement);
// 添加RGB颜色模式
XMLElement* rgbColorPatternElement = doc.NewElement("RgbColorPattern");
rgbColorPatternElement->SetAttribute("r", configResult.algorithmParams.rgbColorPattern.r);
rgbColorPatternElement->SetAttribute("g", configResult.algorithmParams.rgbColorPattern.g);
rgbColorPatternElement->SetAttribute("b", configResult.algorithmParams.rgbColorPattern.b);
algoParamsElement->InsertEndChild(rgbColorPatternElement);
// 添加颜色模板参数
XMLElement* colorTemplateParamElement = doc.NewElement("ColorTemplateParam");
// 保存正面颜色模板
XMLElement* frontColorTemplateElement = doc.NewElement("FrontColorTemplate");
std::string frontColorStr = SerializeColorTemplate(configResult.algorithmParams.colorTemplateParam.frontColorTemplate);
frontColorTemplateElement->SetText(frontColorStr.c_str());
colorTemplateParamElement->InsertEndChild(frontColorTemplateElement);
// 保存反面颜色模板
XMLElement* backColorTemplateElement = doc.NewElement("BackColorTemplate");
std::string backColorStr = SerializeColorTemplate(configResult.algorithmParams.colorTemplateParam.backColorTemplate);
backColorTemplateElement->SetText(backColorStr.c_str());
colorTemplateParamElement->InsertEndChild(backColorTemplateElement);
algoParamsElement->InsertEndChild(colorTemplateParamElement);
// 添加项目类型
XMLElement* projectTypeElement = doc.NewElement("ProjectType");
projectTypeElement->SetAttribute("type", ProjectTypeToString(configResult.projectType).c_str());
root->InsertEndChild(projectTypeElement);
// 添加调试参数（在AlgorithmParams外面）
XMLElement* debugParamElement = doc.NewElement("DebugParam");
debugParamElement->SetAttribute("enableDebug", configResult.debugParam.enableDebug);
debugParamElement->SetAttribute("savePointCloud", configResult.debugParam.savePointCloud);
debugParamElement->SetAttribute("saveDebugImage", configResult.debugParam.saveDebugImage);
debugParamElement->SetAttribute("printDetailLog", configResult.debugParam.printDetailLog);
debugParamElement->SetAttribute("debugOutputPath", configResult.debugParam.debugOutputPath.c_str());
root->InsertEndChild(debugParamElement);
// 添加串口配置
XMLElement* serialConfigElement = doc.NewElement("SerialConfig");
serialConfigElement->SetAttribute("portName", configResult.serialConfig.portName.c_str());
serialConfigElement->SetAttribute("baudRate", configResult.serialConfig.baudRate);
serialConfigElement->SetAttribute("dataBits", configResult.serialConfig.dataBits);
serialConfigElement->SetAttribute("stopBits", configResult.serialConfig.stopBits);
serialConfigElement->SetAttribute("parity", configResult.serialConfig.parity);
serialConfigElement->SetAttribute("flowControl", configResult.serialConfig.flowControl);
serialConfigElement->SetAttribute("enabled", configResult.serialConfig.enabled);
root->InsertEndChild(serialConfigElement);
// 保存到文件
XMLError err = doc.SaveFile(filePath.c_str());
if (err != XML_SUCCESS)
{
std::cerr << "无法保存配置文件: " << filePath << std::endl;
return false;
}
// 触发配置改变通知
if (m_pNotify)
{
m_pNotify->OnConfigChanged(configResult);
}
return true;
}
// 设置配置改变通知回调
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
{
m_pNotify = notify;
}
/**
* @brief 创建实例
* @return 实例
*/
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
{
*ppVrConfig = new CVrConfig() ;
return *ppVrConfig != nullptr;
}
/**
* @brief 解析颜色模板字符串
* @param colorStr 颜色模板字符串，格式为"0.0,0.0,0.0,..."
* @param colorTemplate 输出的颜色模板数组
*/
void CVrConfig::ParseColorTemplate(const std::string& colorStr, double colorTemplate[RGN_HIST_SIZE])
{
std::string str = colorStr;
size_t pos = 0;
int index = 0;
while (pos < str.length() && index < RGN_HIST_SIZE)
{
size_t commaPos = str.find(',', pos);
if (commaPos == std::string::npos)
{
// 最后一个值
colorTemplate[index] = std::stod(str.substr(pos));
break;
}
else
{
colorTemplate[index] = std::stod(str.substr(pos, commaPos - pos));
pos = commaPos + 1;
index++;
}
}
// 如果解析的值不足16个，用0.0填充
for (int i = index + 1; i < RGN_HIST_SIZE; i++)
{
colorTemplate[i] = 0.0;
}
}
/**
* @brief 序列化颜色模板数组为字符串
* @param colorTemplate 颜色模板数组
* @return 序列化后的字符串
*/
std::string CVrConfig::SerializeColorTemplate(const double colorTemplate[RGN_HIST_SIZE])
{
std::string result;
for (int i = 0; i < RGN_HIST_SIZE; i++)
{
if (i > 0) result += ",";
result += std::to_string(colorTemplate[i]);
}
return result;
}
Reference in New Issue Copy Permalink