307 lines
14 KiB
C++
307 lines
14 KiB
C++
#include "ConfigXmlUtils.h"
|
|
#include "VrLog.h"
|
|
#include <cstring>
|
|
#include <cstdio>
|
|
|
|
using namespace tinyxml2;
|
|
|
|
namespace ConfigXmlUtils
|
|
{
|
|
|
|
// ============ 手眼标定矩阵 - 内部辅助 ============
|
|
|
|
static void LoadSingleHandEyeCalibMatrix(XMLElement* handEyeElement,
|
|
VrHandEyeCalibMatrix& calibMatrix,
|
|
int defaultEulerOrder)
|
|
{
|
|
if (handEyeElement->Attribute("cameraIndex"))
|
|
calibMatrix.cameraIndex = handEyeElement->IntAttribute("cameraIndex");
|
|
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
char attrName[32];
|
|
sprintf(attrName, "m%d", i);
|
|
if (handEyeElement->Attribute(attrName))
|
|
calibMatrix.matrix[i] = handEyeElement->DoubleAttribute(attrName);
|
|
}
|
|
|
|
if (handEyeElement->Attribute("eulerOrder"))
|
|
calibMatrix.eulerOrder = handEyeElement->IntAttribute("eulerOrder");
|
|
else
|
|
calibMatrix.eulerOrder = defaultEulerOrder;
|
|
}
|
|
|
|
// ============ 手眼标定矩阵 ============
|
|
|
|
void LoadHandEyeCalibMatrixs(XMLElement* root,
|
|
std::vector<VrHandEyeCalibMatrix>& list,
|
|
int defaultEulerOrder)
|
|
{
|
|
// 优先找 <HandEyeCalibMatrixs>
|
|
XMLElement* containerElement = root->FirstChildElement("HandEyeCalibMatrixs");
|
|
if (!containerElement) {
|
|
// 兼容旧名 <HandEyeCalibMatrixList>
|
|
containerElement = root->FirstChildElement("HandEyeCalibMatrixList");
|
|
}
|
|
|
|
if (containerElement)
|
|
{
|
|
XMLElement* handEyeElement = containerElement->FirstChildElement("HandEyeCalibMatrix");
|
|
while (handEyeElement)
|
|
{
|
|
VrHandEyeCalibMatrix calibMatrix;
|
|
calibMatrix.cameraIndex = static_cast<int>(list.size()) + 1; // 默认递增
|
|
LoadSingleHandEyeCalibMatrix(handEyeElement, calibMatrix, defaultEulerOrder);
|
|
list.push_back(calibMatrix);
|
|
handEyeElement = handEyeElement->NextSiblingElement("HandEyeCalibMatrix");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// 兼容最旧格式：单个 <HandEyeCalibMatrix> 直接在根节点下
|
|
XMLElement* handEyeElement = root->FirstChildElement("HandEyeCalibMatrix");
|
|
if (handEyeElement)
|
|
{
|
|
VrHandEyeCalibMatrix calibMatrix;
|
|
calibMatrix.cameraIndex = 1;
|
|
LoadSingleHandEyeCalibMatrix(handEyeElement, calibMatrix, defaultEulerOrder);
|
|
list.push_back(calibMatrix);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SaveHandEyeCalibMatrixs(XMLDocument& doc,
|
|
XMLElement* root,
|
|
const std::vector<VrHandEyeCalibMatrix>& list)
|
|
{
|
|
XMLElement* containerElement = doc.NewElement("HandEyeCalibMatrixs");
|
|
for (const auto& calibMatrix : list)
|
|
{
|
|
XMLElement* handEyeElement = doc.NewElement("HandEyeCalibMatrix");
|
|
handEyeElement->SetAttribute("cameraIndex", calibMatrix.cameraIndex);
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
char attrName[32];
|
|
sprintf(attrName, "m%d", i);
|
|
handEyeElement->SetAttribute(attrName, calibMatrix.matrix[i]);
|
|
}
|
|
handEyeElement->SetAttribute("eulerOrder", calibMatrix.eulerOrder);
|
|
containerElement->InsertEndChild(handEyeElement);
|
|
}
|
|
root->InsertEndChild(containerElement);
|
|
}
|
|
|
|
// ============ 平面校准参数 ============
|
|
|
|
void LoadPlaneCalibParams(XMLElement* parent, VrPlaneCalibParam& param)
|
|
{
|
|
XMLElement* planeCalibElement = parent->FirstChildElement("PlaneCalibParams");
|
|
if (!planeCalibElement) return;
|
|
|
|
XMLElement* cameraCalibElement = planeCalibElement->FirstChildElement("CameraCalibParam");
|
|
while (cameraCalibElement)
|
|
{
|
|
VrCameraPlaneCalibParam cameraParam;
|
|
|
|
if (cameraCalibElement->Attribute("cameraIndex"))
|
|
cameraParam.cameraIndex = cameraCalibElement->IntAttribute("cameraIndex");
|
|
if (cameraCalibElement->Attribute("cameraName"))
|
|
cameraParam.cameraName = cameraCalibElement->Attribute("cameraName");
|
|
if (cameraCalibElement->Attribute("isCalibrated"))
|
|
cameraParam.isCalibrated = cameraCalibElement->BoolAttribute("isCalibrated");
|
|
if (cameraCalibElement->Attribute("planeHeight"))
|
|
cameraParam.planeHeight = cameraCalibElement->DoubleAttribute("planeHeight");
|
|
|
|
// 旋转矩阵 planeCalib[9] (3x3)
|
|
if (cameraCalibElement->Attribute("planeCalib_00"))
|
|
cameraParam.planeCalib[0] = cameraCalibElement->DoubleAttribute("planeCalib_00");
|
|
if (cameraCalibElement->Attribute("planeCalib_01"))
|
|
cameraParam.planeCalib[1] = cameraCalibElement->DoubleAttribute("planeCalib_01");
|
|
if (cameraCalibElement->Attribute("planeCalib_02"))
|
|
cameraParam.planeCalib[2] = cameraCalibElement->DoubleAttribute("planeCalib_02");
|
|
if (cameraCalibElement->Attribute("planeCalib_10"))
|
|
cameraParam.planeCalib[3] = cameraCalibElement->DoubleAttribute("planeCalib_10");
|
|
if (cameraCalibElement->Attribute("planeCalib_11"))
|
|
cameraParam.planeCalib[4] = cameraCalibElement->DoubleAttribute("planeCalib_11");
|
|
if (cameraCalibElement->Attribute("planeCalib_12"))
|
|
cameraParam.planeCalib[5] = cameraCalibElement->DoubleAttribute("planeCalib_12");
|
|
if (cameraCalibElement->Attribute("planeCalib_20"))
|
|
cameraParam.planeCalib[6] = cameraCalibElement->DoubleAttribute("planeCalib_20");
|
|
if (cameraCalibElement->Attribute("planeCalib_21"))
|
|
cameraParam.planeCalib[7] = cameraCalibElement->DoubleAttribute("planeCalib_21");
|
|
if (cameraCalibElement->Attribute("planeCalib_22"))
|
|
cameraParam.planeCalib[8] = cameraCalibElement->DoubleAttribute("planeCalib_22");
|
|
|
|
// 逆旋转矩阵 invRMatrix[9] (3x3)
|
|
if (cameraCalibElement->Attribute("invRMatrix_00"))
|
|
cameraParam.invRMatrix[0] = cameraCalibElement->DoubleAttribute("invRMatrix_00");
|
|
if (cameraCalibElement->Attribute("invRMatrix_01"))
|
|
cameraParam.invRMatrix[1] = cameraCalibElement->DoubleAttribute("invRMatrix_01");
|
|
if (cameraCalibElement->Attribute("invRMatrix_02"))
|
|
cameraParam.invRMatrix[2] = cameraCalibElement->DoubleAttribute("invRMatrix_02");
|
|
if (cameraCalibElement->Attribute("invRMatrix_10"))
|
|
cameraParam.invRMatrix[3] = cameraCalibElement->DoubleAttribute("invRMatrix_10");
|
|
if (cameraCalibElement->Attribute("invRMatrix_11"))
|
|
cameraParam.invRMatrix[4] = cameraCalibElement->DoubleAttribute("invRMatrix_11");
|
|
if (cameraCalibElement->Attribute("invRMatrix_12"))
|
|
cameraParam.invRMatrix[5] = cameraCalibElement->DoubleAttribute("invRMatrix_12");
|
|
if (cameraCalibElement->Attribute("invRMatrix_20"))
|
|
cameraParam.invRMatrix[6] = cameraCalibElement->DoubleAttribute("invRMatrix_20");
|
|
if (cameraCalibElement->Attribute("invRMatrix_21"))
|
|
cameraParam.invRMatrix[7] = cameraCalibElement->DoubleAttribute("invRMatrix_21");
|
|
if (cameraCalibElement->Attribute("invRMatrix_22"))
|
|
cameraParam.invRMatrix[8] = cameraCalibElement->DoubleAttribute("invRMatrix_22");
|
|
|
|
param.SetCameraCalibParam(cameraParam);
|
|
|
|
cameraCalibElement = cameraCalibElement->NextSiblingElement("CameraCalibParam");
|
|
}
|
|
}
|
|
|
|
void SavePlaneCalibParams(XMLDocument& doc, XMLElement* parent,
|
|
const VrPlaneCalibParam& param)
|
|
{
|
|
XMLElement* planeCalibElement = doc.NewElement("PlaneCalibParams");
|
|
|
|
for (const auto& cameraParam : param.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]);
|
|
|
|
planeCalibElement->InsertEndChild(cameraCalibElement);
|
|
}
|
|
|
|
parent->InsertEndChild(planeCalibElement);
|
|
}
|
|
|
|
// ============ 调试参数 ============
|
|
|
|
void LoadDebugParam(XMLElement* root, VrDebugParam& param)
|
|
{
|
|
XMLElement* debugElement = root->FirstChildElement("DebugParam");
|
|
if (!debugElement) return;
|
|
|
|
if (debugElement->Attribute("enableDebug"))
|
|
param.enableDebug = debugElement->BoolAttribute("enableDebug");
|
|
if (debugElement->Attribute("savePointCloud"))
|
|
param.savePointCloud = debugElement->BoolAttribute("savePointCloud");
|
|
if (debugElement->Attribute("saveDebugImage"))
|
|
param.saveDebugImage = debugElement->BoolAttribute("saveDebugImage");
|
|
if (debugElement->Attribute("printDetailLog"))
|
|
param.printDetailLog = debugElement->BoolAttribute("printDetailLog");
|
|
if (debugElement->Attribute("debugOutputPath"))
|
|
param.debugOutputPath = debugElement->Attribute("debugOutputPath");
|
|
}
|
|
|
|
void SaveDebugParam(XMLDocument& doc, XMLElement* root, const VrDebugParam& param)
|
|
{
|
|
XMLElement* debugElement = doc.NewElement("DebugParam");
|
|
debugElement->SetAttribute("enableDebug", param.enableDebug);
|
|
debugElement->SetAttribute("savePointCloud", param.savePointCloud);
|
|
debugElement->SetAttribute("saveDebugImage", param.saveDebugImage);
|
|
debugElement->SetAttribute("printDetailLog", param.printDetailLog);
|
|
debugElement->SetAttribute("debugOutputPath", param.debugOutputPath.c_str());
|
|
root->InsertEndChild(debugElement);
|
|
}
|
|
|
|
// ============ 串口配置 ============
|
|
|
|
void LoadSerialConfig(XMLElement* root, SerialConfig& config)
|
|
{
|
|
XMLElement* serialElement = root->FirstChildElement("SerialConfig");
|
|
if (!serialElement) return;
|
|
|
|
if (serialElement->Attribute("portName"))
|
|
config.portName = serialElement->Attribute("portName");
|
|
if (serialElement->Attribute("baudRate"))
|
|
config.baudRate = serialElement->IntAttribute("baudRate");
|
|
if (serialElement->Attribute("dataBits"))
|
|
config.dataBits = serialElement->IntAttribute("dataBits");
|
|
if (serialElement->Attribute("stopBits"))
|
|
config.stopBits = serialElement->IntAttribute("stopBits");
|
|
if (serialElement->Attribute("parity"))
|
|
config.parity = serialElement->IntAttribute("parity");
|
|
if (serialElement->Attribute("flowControl"))
|
|
config.flowControl = serialElement->IntAttribute("flowControl");
|
|
if (serialElement->Attribute("enabled"))
|
|
config.enabled = serialElement->BoolAttribute("enabled");
|
|
}
|
|
|
|
void SaveSerialConfig(XMLDocument& doc, XMLElement* root, const SerialConfig& config)
|
|
{
|
|
XMLElement* serialElement = doc.NewElement("SerialConfig");
|
|
serialElement->SetAttribute("portName", config.portName.c_str());
|
|
serialElement->SetAttribute("baudRate", config.baudRate);
|
|
serialElement->SetAttribute("dataBits", config.dataBits);
|
|
serialElement->SetAttribute("stopBits", config.stopBits);
|
|
serialElement->SetAttribute("parity", config.parity);
|
|
serialElement->SetAttribute("flowControl", config.flowControl);
|
|
serialElement->SetAttribute("enabled", config.enabled);
|
|
root->InsertEndChild(serialElement);
|
|
}
|
|
|
|
// ============ 相机列表 ============
|
|
|
|
void LoadCameraList(XMLElement* root, std::vector<DeviceInfo>& list)
|
|
{
|
|
XMLElement* camerasElement = root->FirstChildElement("Cameras");
|
|
if (!camerasElement) return;
|
|
|
|
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");
|
|
if (cameraElement->Attribute("index"))
|
|
camera.index = cameraElement->IntAttribute("index");
|
|
|
|
list.push_back(camera);
|
|
cameraElement = cameraElement->NextSiblingElement("Camera");
|
|
}
|
|
}
|
|
|
|
void SaveCameraList(XMLDocument& doc, XMLElement* root, const std::vector<DeviceInfo>& list)
|
|
{
|
|
XMLElement* camerasElement = doc.NewElement("Cameras");
|
|
for (const auto& camera : list)
|
|
{
|
|
XMLElement* cameraElement = doc.NewElement("Camera");
|
|
cameraElement->SetAttribute("name", camera.name.c_str());
|
|
cameraElement->SetAttribute("ip", camera.ip.c_str());
|
|
if (camera.index > 0)
|
|
cameraElement->SetAttribute("index", camera.index);
|
|
camerasElement->InsertEndChild(cameraElement);
|
|
}
|
|
root->InsertEndChild(camerasElement);
|
|
}
|
|
|
|
} // namespace ConfigXmlUtils
|